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