generate.py

Go to the documentation of this file.

# Software License Agreement (BSD License)
#
# Copyright (c) 2014, JSK Robotics Laboratory.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of JSK Robotics Laboratory. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#

## ROS message source code generation for EusLisp(roseus)


import sys
import os
import traceback
import re

from genmsg import SrvSpec, MsgSpec, MsgContext
from genmsg.msg_loader import load_srv_from_file, load_msg_by_type
import genmsg.gentools

try:
    from cStringIO import StringIO #Python 2.x
except ImportError:
    from io import StringIO #Python 3.x

############################################################
# Built in types
############################################################

def is_integer(t):
    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

def is_signed_int(t):
    return t in ['byte', 'int8', 'int16', 'int32', 'int64']

def is_unsigned_int(t):
    return t in ['char', 'uint8', 'uint16', 'uint32', 'uint64']

def is_bool(t):
    return t == 'bool'

def is_string(t):
    return t == 'string'

def is_float(t):
    return t in ['float16', 'float32', 'float64']

def is_time(t):
    return t in ['time', 'duration']

def field_type(f):
    if f.is_builtin:
        elt_type = lisp_type(f.base_type, f.is_array)
    else:
        elt_type = msg_type(f)
    return elt_type

def parse_msg_type(f):
    "returns (package, msg_or_srv)"
    if f.base_type == 'Header':
        return ('std_msgs', 'Header')
    else:
        return f.base_type.split('/')

def msg_type(f):
    "returns roseus namespace package::msg_or_srv"
    (pkg, msg) = parse_msg_type(f)
    return '%s::%s'%(pkg, msg)

def lisp_type(t, array):
    if t == 'uint8' and array:
        return 'char'
    if is_integer(t):
        return 'integer'
    elif is_bool(t):
        return 'object'
    elif is_float(t):
        return 'float'
    elif is_time(t):
        return 'ros::time'
    elif is_string(t):
        return 'string'
    else:
        raise ValueError('%s is not a recognized primitive type'%t)

def field_initform(f):
    var = "__%s"%f.name
    if f.is_builtin and not f.is_array:
        if is_integer(f.base_type):
            return '(round %s)'%var
        elif is_float(f.base_type):
            return '(float %s)'%var
        elif is_string(f.base_type):
            return '(string %s)'%var
    return var

def field_initvalue(f):
    initvalue = lisp_initvalue(f.base_type)
    elt_type = lisp_type(f.base_type, f.is_array)
    if not is_time(f.base_type):
        elt_type = ':'+elt_type
    if f.is_array:
        len = f.array_len or 0
        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):
            return '(make-array %s :initial-element %s :element-type %s)'%(len, initvalue, elt_type)
        else:
            return '(let (r) (dotimes (i %s) (push %s r)) r)'%(len, initvalue)
    else:
        return initvalue

def lisp_initvalue(t):
    if is_integer(t):
        return '0'
    elif is_bool(t):
        return 'nil'
    elif is_float(t):
        return '0.0'
    elif is_time(t):
        return '(instance ros::time :init)'
    elif is_string(t):
        return '\"\"'
    else:
        raise ValueError('%s is not a recognized primitive type'%t)

def lisp_initform(t):
    if is_integer(t):
        return 'round'
    elif is_bool(t):
        return 'nil'
    elif is_float(t):
        return 'float'
    elif is_time(t):
        return 'ros::time'
    elif is_string(t):
        return 'string'
    else:
        raise ValueError('%s is not a recognized primitive type'%t)

NUM_BYTES = {'byte': 1, 'int8': 1, 'int16': 2, 'int32': 4, 'int64': 8,
             'char': 1, 'uint8': 1, 'uint16': 2, 'uint32': 4, 'uint64': 8}
             
             

############################################################
# Indented writer
############################################################

class IndentedWriter():

    def __init__(self, s):
        self.str = s
        self.indentation = 0
        self.block_indent = False

    def write(self, s, indent=True, newline=True):
        if not indent:
            newline = False
        if self.block_indent:
            self.block_indent = False
        else:
            if newline:
                self.str.write('\n')
            if indent:
                for i in range(self.indentation):
                    self.str.write(' ')
        self.str.write(s)

    def newline(self):
        self.str.write('\n')

    def inc_indent(self, inc=2):
        self.indentation += inc

    def dec_indent(self, dec=2):
        self.indentation -= dec

    def reset_indent(self):
        self.indentation = 0

    def block_next_indent(self):
        self.block_indent = True

class Indent():

    def __init__(self, w, inc=2, indent_first=True):
        self.writer = w
        self.inc = inc
        self.indent_first = indent_first

    def __enter__(self):
        self.writer.inc_indent(self.inc)
        if not self.indent_first:
            self.writer.block_next_indent()

    def __exit__(self, type, val, traceback):
        self.writer.dec_indent(self.inc)

def write_begin(s, spec, is_service=False):
    "Writes the beginning of the file: a comment saying it's auto-generated and the in-package form"

    s.write(';; Auto-generated. Do not edit!\n\n', newline=False)
    suffix = 'srv' if is_service else 'msg'
    if is_service:
        spec.actual_name=spec.short_name
    s.write('(when (boundp \'%s::%s)'%(spec.package, spec.actual_name))
    s.write('  (if (not (find-package "%s"))'%(spec.package.upper()))
    s.write('    (make-package "%s"))'%(spec.package.upper()))
    s.write('  (shadow \'%s (find-package "%s")))'%(spec.actual_name, spec.package.upper()))
    s.write('(unless (find-package "%s::%s")'%(spec.package.upper(), spec.actual_name.upper()))
    s.write('  (make-package "%s::%s"))'%(spec.package.upper(), spec.actual_name.upper()))
    if is_service:
        s.write('(unless (find-package "%s::%sREQUEST")'%(spec.package.upper(), spec.actual_name.upper()))
        s.write('  (make-package "%s::%sREQUEST"))'%(spec.package.upper(), spec.actual_name.upper()))
        s.write('(unless (find-package "%s::%sRESPONSE")'%(spec.package.upper(), spec.actual_name.upper()))
        s.write('  (make-package "%s::%sRESPONSE"))'%(spec.package.upper(), spec.actual_name.upper()))
    s.write('')
    s.write('(in-package "ROS")')
    s.newline()

def write_include(s, spec, is_srv=False):
    if not is_srv:
        s.write(';;//! \\htmlinclude %s.msg.html'%spec.actual_name, newline=False) # t2
    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])):
        s.write('(if (not (find-package "%s"))'%msg_type.upper())
        s.write('  (ros::roseus-add-msgs "%s"))'%msg_type)
    s.newline()
    s.newline()

def write_slot_definition(s, field):
    "Write the definition of a slot corresponding to a single message field"
    s.write('_%s '%field.name, indent=False, newline=False)


def write_slot_argument(s, field):
    "Write the key arguments of a slot corresponding to a single message field"
    var = field.name
    if field.is_builtin:
        s.write('((:%s __%s) %s)'%(var, var, field_initvalue(field)))
    else:
        if field.is_array:
            len = field.array_len or 0
            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
        else:
            s.write('((:%s __%s) (instance %s :init))'%(var, var, field_type(field)))

def write_slot_initialize(s, field):
    "Write the initialization of a slot corresponding to a single message field"
    s.write('(setq _%s %s)'%(field.name, field_initform(field)))

def write_defclass(s, spec):
    "Writes the defclass that defines the message type"
    s.write('(defclass %s::%s'%(spec.package, spec.actual_name))
    with Indent(s):
        s.write(':super ros::object')
        s.write(':slots (')
        with Indent(s, inc=1, indent_first=False):
            for field in spec.parsed_fields():
                write_slot_definition(s, field)
        s.write('))', indent=False)
    s.newline()

def write_defmethod(s, spec):
    s.write('(defmethod %s::%s'%(spec.package, spec.actual_name))
    with Indent(s):
        s.write('(:init')
        with Indent(s, inc=1):
            s.write('(&key')
            with Indent(s, inc=1):
                for field in spec.parsed_fields():
                    write_slot_argument(s, field)
                s.write(')')
            s.write('(send-super :init)')
            for field in spec.parsed_fields():
                write_slot_initialize(s, field)
            s.write('self)')


def write_accessors(s, spec):
    with Indent(s):
        for field in spec.parsed_fields():
            s.write('(:%s'%field.name)
            var = '_%s'%field.name
            with Indent(s, inc=1):
                if field.is_builtin:
                    if field.type == "bool":
                        s.write('(&optional (_%s :null))'%var)
                        s.write('(if (not (eq _%s :null)) (setq %s _%s)) %s)'%(var,var,var,var))
                    else:
                        s.write('(&optional _%s)'%var)
                        s.write('(if _%s (setq %s _%s)) %s)'%(var,var,var,var))
                else:
                    s.write('(&rest _%s)'%var)
                    s.write('(if (keywordp (car _%s))'%var)
                    s.write('    (send* %s _%s)'%(var,var))
                    with Indent(s, inc=2):
                        s.write('(progn')
                        s.write('  (if _%s (setq %s (car _%s)))'%(var,var,var))
                        s.write('  %s)))'%var)

def write_serialize_length(s, v, is_array=False):
    if is_array:
        s.write('(write-long (length %s) s)'%(v))
    else:
        s.write('(write-long (length %s) s) (princ %s s)'%(v,v))


def write_serialize_bits(s, v, num_bytes): 
    if num_bytes == 1:
        s.write('(write-byte %s s)'%v)
    elif num_bytes == 2:
        s.write('(write-word %s s)'%v)
    elif num_bytes == 4:
        s.write('(write-long %s s)'%v)
    else:
        s.write('\n', indent=False)
        s.write('#+(or :alpha :irix6 :x86_64)', indent=False, newline=False)
        s.write('(progn (sys::poke %s (send s :buffer) (send s :count) :long) (incf (stream-count s) 8))'%v)
        s.write('\n', indent=False)
        s.write('#-(or :alpha :irix6 :x86_64)', indent=False)
        s.write('(cond ((and (class %s) (= (length (%s . bv)) 2)) ;; bignum'%(v,v))
        s.write('       (write-long (ash (elt (%s . bv) 0) 0) s)'%v)
        s.write('       (write-long (ash (elt (%s . bv) 1) -1) s))'%v)
        s.write('      ((and (class %s) (= (length (%s . bv)) 1)) ;; big1'%(v,v))
        s.write('       (write-long (elt (%s . bv) 0) s)'%v)
        s.write('       (write-long (if (>= %s 0) 0 #xffffffff) s))'%v)
        s.write('      (t                                         ;; integer')
        s.write('       (write-long %s s)(write-long (if (>= %s 0) 0 #xffffffff) s)))'%(v,v))


def write_serialize_bits_signed(s, v, num_bytes):
    write_serialize_bits(s, v, num_bytes)

def write_serialize_builtin(s, f, v):
    if f.base_type == 'string':
        write_serialize_length(s, v)
    elif f.base_type == 'float32':
        s.write('(sys::poke %s (send s :buffer) (send s :count) :float) (incf (stream-count s) 4)'%v)
    elif f.base_type == 'float64':
        s.write('(sys::poke %s (send s :buffer) (send s :count) :double) (incf (stream-count s) 8)'%v)
    elif f.base_type == 'bool':
        s.write('(if %s (write-byte -1 s) (write-byte 0 s))'%v)
    elif f.base_type in ['byte', 'char']:
        s.write('(write-byte %s s)'%v)
    elif f.base_type in ['duration', 'time']:
        s.write('(write-long (send %s :sec) s) (write-long (send %s :nsec) s)'%(v,v))
    elif is_signed_int(f.base_type):
        write_serialize_bits_signed(s, v, NUM_BYTES[f.base_type])
    elif is_unsigned_int(f.base_type):
        write_serialize_bits(s, v, NUM_BYTES[f.base_type])
    else:
        raise ValueError('Unknown type: %s', f.base_type)

def write_serialize_field(s, f):
    s.write(';; %s _%s'%(f.type, f.name))
    slot = '_%s'%f.name
    var = slot
    if f.is_array and f.base_type == 'uint8':
        if not f.array_len:
            s.write('(write-long (length %s) s)'%slot)
        s.write('(princ %s s)'%slot)
    elif f.is_array and is_string(f.base_type):
        s.write('(write-long (length %s) s)'%slot)
        s.write('(dolist (elem %s)'%slot)
        var = 'elem'
    elif f.is_array:
        if not f.array_len:
            write_serialize_length(s, slot, True)
        if f.is_builtin and f.array_len:
            s.write('(dotimes (i %s)'%f.array_len)
        elif f.is_builtin and not f.array_len:
            s.write('(dotimes (i (length %s))'%var)
        else:
            s.write('(dolist (elem %s)'%slot)
        slot = 'elem'
        var = '(elt %s i)'%var
        s.block_next_indent()
        s.write('')

    if f.is_array and f.base_type == 'uint8':
        pass
    elif f.is_builtin:
        with Indent(s):
            write_serialize_builtin(s, f, var)
    else:
        with Indent(s):
            s.write('(send %s :serialize s)'%slot)

    if f.is_array and f.base_type != 'uint8':
        s.write('  )')
 
def write_serialize(s, spec):
    """
    Write the serialize method
    """
    with Indent(s):
        s.write('(:serialize')
        with Indent(s,inc=1):
            s.write('(&optional strm)')
            s.write('(let ((s (if strm strm')
            s.write('           (make-string-output-stream (send self :serialization-length)))))')
            with Indent(s):
                for f in spec.parsed_fields():
                    write_serialize_field(s, f)
                s.write(';;')
                s.write('(if (null strm) (get-output-stream-string s))))')

def write_deserialize_length(s, f, v, is_array=False):
    if is_array:
        s.write('(let (n)') ## TODO should not be here
        with Indent(s):
            s.write('(setq n (sys::peek buf ptr- :integer)) (incf ptr- 4)')
            s.write('(setq %s (let (r) (dotimes (i n) (push (instance %s :init) r)) r))'%(v,field_type(f)))
    else:
        set = 'setf' if v[0] == '(' else 'setq'
        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))

def write_deserialize_bits(s, v, num_bytes):
    if num_bytes == 1:
        type = ':char'
    elif num_bytes == 2:
        type = ':short'
    elif num_bytes == 4:
        type = ':integer'
    elif num_bytes == 8:
        type = ':long'
        s.write('')
        return write_deserialize_bits_signed(s,v,num_bytes)
    else:
        raise ValueError('Unknown size: %s', num_bytes)

    set = 'setf' if v[0] == '(' else 'setq'
    s.write('(%s %s (sys::peek buf ptr- %s)) (incf ptr- %s)'%(set,v,type,num_bytes))

def write_deserialize_bits_signed(s, v, num_bytes):
    if num_bytes in [1,2,4]:
        write_deserialize_bits(s, v, num_bytes)
    else:
        s.write('\n', indent=False)
        s.write('#+(or :alpha :irix6 :x86_64)', indent=False)
        s.write(' (setf %s (prog1 (sys::peek buf ptr- :long) (incf ptr- 8)))\n'%v)
        s.write('#-(or :alpha :irix6 :x86_64)', indent=False)
        s.write(' (setf %s (let ((b0 (prog1 (sys::peek buf ptr- :integer) (incf ptr- 4)))'%v)
        s.write('             (b1 (prog1 (sys::peek buf ptr- :integer) (incf ptr- 4))))')
        s.write('         (cond ((= b1 -1) b0)')
        s.write('                ((and (= b1  0)')
        s.write('                      (<= lisp::most-negative-fixnum b0 lisp::most-positive-fixnum))')
        s.write('                 b0)')
        s.write('               ((= b1  0) (make-instance bignum :size 1 :bv (integer-vector b0)))')
        s.write('               (t (make-instance bignum :size 2 :bv (integer-vector b0 (ash b1 1)))))))')

def write_deserialize_builtin(s, f, v):
    set = 'setf' if v[0] == '(' else 'setq'
    if f.base_type == 'string':
        write_deserialize_length(s,f,v)
    elif f.base_type == 'float32':
        s.write('(%s %s (sys::peek buf ptr- :float)) (incf ptr- 4)'%(set, v))
    elif f.base_type == 'float64':
        s.write('(%s %s (sys::peek buf ptr- :double)) (incf ptr- 8)'%(set, v))
    elif f.base_type == 'bool':
        s.write('(%s %s (not (= 0 (sys::peek buf ptr- :char)))) (incf ptr- 1)'%(set, v))
    elif f.base_type in ['duration', 'time']:
        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))
    elif is_signed_int(f.base_type):
        write_deserialize_bits_signed(s, v, NUM_BYTES[f.base_type])
        if NUM_BYTES[f.base_type] == 1: # if signed byte, we have to convert to -128-127
            s.write('(if (> %s 127) (%s %s (- %s 256)))'%(v,set,v,v))
    elif is_unsigned_int(f.base_type):
        write_deserialize_bits(s, v, NUM_BYTES[f.base_type])
    else:
        raise ValueError('%s unknown'%f.base_type)

def write_deserialize_field(s, f, pkg):
    var = '_%s'%f.name
    s.write(';; %s %s'%(f.type, var))
    if f.is_array:
        if f.is_builtin:
            if f.base_type == 'uint8':
                if f.array_len:
                    s.write('(setq %s (make-array %d :element-type :char))'%(var,f.array_len))
                    s.write('(replace %s buf :start2 ptr-) (incf ptr- %d)'%(var,f.array_len))
                else:
                    s.write('(let ((n (sys::peek buf ptr- :integer))) (incf ptr- 4)')
                    s.write('  (setq %s (make-array n :element-type :char))'%var)
                    s.write('  (replace %s buf :start2 ptr-) (incf ptr- n))'%(var))
            elif f.array_len:
                s.write('(dotimes (i (length %s))'%var)
                var = '(elt %s i)'%var
            else:
                if is_float(f.base_type) or is_integer(f.base_type) or is_string(f.base_type) or is_bool(f.base_type):
                    s.write('(let (n)')
                    with Indent(s):
                        s.write('(setq n (sys::peek buf ptr- :integer)) (incf ptr- 4)')
                        if is_string(f.base_type) or is_bool(f.base_type):
                            s.write('(setq %s (make-list n))'%var)
                        else:
                            s.write('(setq %s (instantiate %s-vector n))'%(var, lisp_type(f.base_type, f.is_array)))
                        s.write('(dotimes (i n)')
                        var = '(elt %s i)'%var
                else:
                    write_deserialize_length(s, f, var, True)
                    var = 'elem-'
                    with Indent(s):
                        s.write('(dolist (%s _%s)'%(var, f.name))
        else: # array but not builtin
            if f.array_len:
                s.write('(dotimes (i %s)'%f.array_len)
                var = '(elt _%s i)'%f.name
            else:
                write_deserialize_length(s, f, var, True)
                var = 'elem-'
                with Indent(s):
                    s.write('(dolist (%s _%s)'%(var, f.name))
    if f.is_array and f.base_type == 'uint8':
        pass
    elif f.is_builtin:
        with Indent(s):
            write_deserialize_builtin(s, f, var)
    else:
        with Indent(s):
            s.write('(send %s :deserialize buf ptr-) (incf ptr- (send %s :serialization-length))'%(var, var))

    if f.is_array and not f.base_type == 'uint8':
        with Indent(s):
            if f.array_len:
                s.write(')')
            else:
                s.write('))')


def write_deserialize(s, spec):
    """
    Write the deserialize method
    """
    with Indent(s):
        s.write('(:deserialize')
        with Indent(s,inc=1):
            s.write('(buf &optional (ptr- 0))')
            for f in spec.parsed_fields():
                write_deserialize_field(s, f, spec.package)
            s.write(';;')
            s.write('self)')
        s.write(')')
        s.newline()

def write_md5sum(s, msg_context, spec, parent=None):
    md5sum = genmsg.compute_md5(msg_context, parent or spec)
    s.write('(setf (get %s::%s :md5sum-) "%s")'%(spec.package, spec.actual_name, md5sum))

def write_ros_datatype(s, spec):
    s.write('(setf (get %s::%s :datatype-) "%s/%s")'%(spec.package, spec.actual_name, spec.package, spec.actual_name))

def write_message_definition(s, msg_context, spec):
    s.write('(setf (get %s::%s :definition-)'%(spec.package, spec.actual_name))
    with Indent(s,6):
        s.write('"')
        definition = genmsg.compute_full_text(msg_context, spec)
        lines = definition.split('\n')
        for line in lines:
            l = line.replace('\\', '\\\\')
            l = l.replace('"', '\\"')
            s.write('%s\n'%l, indent=False, newline=False)
    s.write('")', newline=False)
    s.write('\n\n')

def write_service_definition(s, msg_context, spec, parent):
    s.write('(setf (get %s::%s :definition-)'%(parent.package, parent.actual_name))
    with Indent(s,6):
        s.write('"')
        for spec_service in [spec.request, spec.response]:
            definition = genmsg.compute_full_text(msg_context, spec_service)
            lines = definition.split('\n')
            for line in lines[:-1]:
                l = line.replace('\\', '\\\\')
                l = l.replace('"', '\\"')
                s.write('%s\n'%l, indent=False, newline=False)
            if spec_service == spec.request:
                s.write('---\n', indent=False, newline=False)
    s.write('")', newline=False)

def write_builtin_length(s, f, var='msg'):
    if f.base_type in ['int8', 'uint8']:
        s.write('1')
    elif f.base_type in ['int16', 'uint16']:
        s.write('2')
    elif f.base_type in ['int32', 'uint32', 'float32']:
        s.write('4')
    elif f.base_type in ['int64', 'uint64', 'float64', 'duration', 'time']:
        s.write('8')
    elif f.base_type == 'string':
        s.write('4 (length _%s)'%f.name)
    elif f.base_type in ['bool', 'byte', 'char']:
        s.write('1')
    else:
        raise ValueError('Unknown: %s', f.base_type)

def write_serialization_length(s, spec):
    with Indent(s):
        s.write('(:serialization-length')
        with Indent(s, inc=1):
            s.write('()')
            s.write('(+')
            with Indent(s, 1):
                if not spec.parsed_fields():
                    s.write('0')
                for field in spec.parsed_fields():
                    s.write(';; %s _%s'%(field.type, field.name))
                    if field.is_array:
                        if field.is_builtin and not is_string(field.base_type):
                            s.write('(* ')
                        else:
                            s.write('(apply #\'+ ')
                        s.block_next_indent()

                        if field.is_builtin:
                            if not field.array_len:
                                if is_string(field.base_type):
                                    s.write('(mapcar #\'(lambda (x) (+ 4 (length x))) _%s)) 4'%(field.name))
                                else:
                                    write_builtin_length(s, field)
                                    s.write('(length _%s)) 4'%field.name, newline=False)
                            else:
                                write_builtin_length(s, field)
                                s.write('%s)'%field.array_len, newline=False)
                        else:
                            if field.array_len:
                                s.write('(send-all _%s :serialization-length))'%field.name)
                            else:
                                s.write('(send-all _%s :serialization-length)) 4'%field.name)
                    else:
                        if field.is_builtin:
                            write_builtin_length(s, field)
                        else:
                            s.write('(send _%s :serialization-length)'%field.name)

                s.write('))')


def write_provide(s, msg_context, spec):
    md5sum = genmsg.compute_md5(msg_context, spec)
    s.write('(provide :%s/%s "%s")'%(spec.package, spec.actual_name,md5sum))
    s.write('\n')

def write_constants(s, spec):
    if spec.constants:
        for c in spec.constants:
            s.write('(intern "*%s*" (find-package "%s::%s"))'%(c.name.upper(), spec.package.upper(), spec.actual_name.upper()))
            s.write('(shadow \'*%s* (find-package "%s::%s"))'%(c.name.upper(), spec.package.upper(), spec.actual_name.upper()))
            if c.type == 'string':
                s.write('(defconstant %s::%s::*%s* "%s")'%(spec.package, spec.actual_name, c.name.upper(), c.val.replace('"', '\\"')))
            elif c.type == 'bool':
                s.write('(defconstant %s::%s::*%s* %s)'%(spec.package, spec.actual_name, c.name.upper(), "t" if c.val == True else "nil"))
            else:
                s.write('(defconstant %s::%s::*%s* %s)'%(spec.package, spec.actual_name, c.name.upper(), c.val))

def write_srv_component(s, spec, context, parent):
    spec.component_type='service'
    write_constants(s, spec)
    write_defclass(s, spec)
    write_defmethod(s, spec)
    write_accessors(s, spec)
    write_serialization_length(s, spec)
    write_serialize(s, spec)
    write_deserialize(s, spec)

def write_service_specific_methods(s, context, spec):
    ### this should be move to previsou definition section ???
    s.write('(defclass %s::%s'%(spec.package, spec.actual_name))
    with Indent(s):
        s.write(':super ros::object')
        s.write(':slots ())')
    s.newline()
    write_md5sum(s, context, spec, parent=spec)
    write_ros_datatype(s, spec)
    s.write('(setf (get %s::%s :request) %s::%s)'%(spec.package, spec.actual_name, spec.request.package, spec.request.actual_name))
    s.write('(setf (get %s::%s :response) %s::%s)'%(spec.package, spec.actual_name, spec.response.package, spec.response.actual_name))
    s.newline()
    s.write('(defmethod %s::%s'%(spec.request.package, spec.request.actual_name))
    s.write('  (:response () (instance %s::%s :init)))'%(spec.response.package, spec.response.actual_name))
    s.newline()
    for spec_service in [spec.request, spec.response]:
        write_md5sum(s, context, spec_service, parent=spec)
        write_ros_datatype(s, spec_service)
        write_service_definition(s, context, spec, spec_service)
        s.newline()
    s.write('\n')
    write_provide(s, context, spec)
    s.write('\n', newline=False)

def generate_msg(pkg, files, out_dir, search_path):
    """
    Generate euslisp code for all messages in a package
    """
    msg_context = MsgContext.create_default()
    for f in files:
        f = os.path.abspath(f)
        infile = os.path.basename(f)
        full_type = genmsg.gentools.compute_full_type_name(pkg, infile)
        spec = genmsg.msg_loader.load_msg_from_file(msg_context, f, full_type)
        generate_msg_from_spec(msg_context, spec, search_path, out_dir, pkg)

def generate_srv(pkg, files, out_dir, search_path):
    """
    Generate euslisp code for all services in a package
    """
    msg_context = MsgContext.create_default()
    for f in files:
        f = os.path.abspath(f)
        infile = os.path.basename(f)
        full_type = genmsg.gentools.compute_full_type_name(pkg, infile)
        spec = genmsg.msg_loader.load_srv_from_file(msg_context, f, full_type)
        generate_srv_from_spec(msg_context, spec, search_path, out_dir, pkg, f)

def msg_list(pkg, search_path, ext):
    dir_list = search_path[pkg]
    files = []
    for d in dir_list:
        files.extend([f for f in os.listdir(d) if f.endswith(ext)])
    return [f[:-len(ext)] for f in files]

def generate_msg_from_spec(msg_context, spec, search_path, output_dir, package):
    """
    Generate a message
    
    @param msg_path: The path to the .msg file
    @type msg_path: str
    """
    genmsg.msg_loader.load_depends(msg_context, spec, search_path)
    spec.actual_name=spec.short_name
    spec.component_type='message'
    msgs = msg_list(package, search_path, '.msg')
    for m in msgs:
        genmsg.load_msg_by_type(msg_context, '%s/%s'%(package, m), search_path)
    

    ########################################
    # 1. Write the .l file
    ########################################
    
    io = StringIO()
    s =  IndentedWriter(io)
    write_begin(s, spec)
    write_include(s, spec)
    write_constants(s, spec)
    write_defclass(s, spec)
    write_defmethod(s, spec)
    write_accessors(s, spec)
    write_serialization_length(s, spec)
    write_serialize(s, spec)
    write_deserialize(s, spec)
    write_md5sum(s, msg_context, spec)
    write_ros_datatype(s, spec)
    write_message_definition(s, msg_context, spec)
    write_provide(s, msg_context, spec)
    
    if (not os.path.exists(output_dir)):
        # if we're being run concurrently, the above test can report false but os.makedirs can still fail if
        # another copy just created the directory
        try:
            os.makedirs(output_dir)
        except OSError as e:
            pass

    with open('%s/%s.l'%(output_dir, spec.short_name), 'w') as f:
        f.write(io.getvalue() + "\n")
    io.close()


# t0 most of this could probably be refactored into being shared with messages
def generate_srv_from_spec(msg_context, spec, search_path, output_dir, package, path):
    "Generate code from .srv file"
    genmsg.msg_loader.load_depends(msg_context, spec, search_path)
    ext = '.srv'
    srvs = [f[:-len(ext)] for f in os.listdir(os.path.dirname(path)) if f.endswith(ext)]
    for s in srvs:
        load_srv_from_file(msg_context, path, '%s/%s'%(package, s))

    ########################################
    # 1. Write the .l file
    ########################################

    io = StringIO()
    s = IndentedWriter(io)
    write_begin(s, spec, True)
    write_include(s, spec.request, is_srv=True)
    write_include(s, spec.response, is_srv=True)
    spec.request.actual_name='%sRequest'%spec.short_name
    spec.response.actual_name='%sResponse'%spec.short_name
    write_srv_component(s, spec.request, msg_context, spec)
    write_srv_component(s, spec.response, msg_context, spec)
    write_service_specific_methods(s, msg_context, spec)

    with open('%s/%s.l'%(output_dir, spec.short_name), 'w') as f:
        f.write(io.getvalue())
    io.close()