00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 import sys
00043 import os
00044 import traceback
00045 import re
00046
00047
00048
00049
00050
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
00057 except ImportError:
00058 from io import StringIO
00059
00060
00061
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']
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
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
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)
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
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
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
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
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
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)
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(')')
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
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
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
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
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
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
00779
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
00791
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
00803
00804
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
00816
00817
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
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
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
00857
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
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
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