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 if field.type == "bool":
00312 s.write('(&optional (_%s :null))'%var)
00313 s.write('(if (not (eq _%s :null)) (setq %s _%s)) %s)'%(var,var,var,var))
00314 else:
00315 s.write('(&optional _%s)'%var)
00316 s.write('(if _%s (setq %s _%s)) %s)'%(var,var,var,var))
00317 else:
00318 s.write('(&rest _%s)'%var)
00319 s.write('(if (keywordp (car _%s))'%var)
00320 s.write(' (send* %s _%s)'%(var,var))
00321 with Indent(s, inc=2):
00322 s.write('(progn')
00323 s.write(' (if _%s (setq %s (car _%s)))'%(var,var,var))
00324 s.write(' %s)))'%var)
00325
00326 def write_serialize_length(s, v, is_array=False):
00327 if is_array:
00328 s.write('(write-long (length %s) s)'%(v))
00329 else:
00330 s.write('(write-long (length %s) s) (princ %s s)'%(v,v))
00331
00332
00333 def write_serialize_bits(s, v, num_bytes):
00334 if num_bytes == 1:
00335 s.write('(write-byte %s s)'%v)
00336 elif num_bytes == 2:
00337 s.write('(write-word %s s)'%v)
00338 elif num_bytes == 4:
00339 s.write('(write-long %s s)'%v)
00340 else:
00341 s.write('\n', indent=False)
00342 s.write('#+(or :alpha :irix6 :x86_64)', indent=False, newline=False)
00343 s.write('(progn (sys::poke %s (send s :buffer) (send s :count) :long) (incf (stream-count s) 8))'%v)
00344 s.write('\n', indent=False)
00345 s.write('#-(or :alpha :irix6 :x86_64)', indent=False)
00346 s.write('(cond ((and (class %s) (= (length (%s . bv)) 2)) ;; bignum'%(v,v))
00347 s.write(' (write-long (ash (elt (%s . bv) 0) 0) s)'%v)
00348 s.write(' (write-long (ash (elt (%s . bv) 1) -1) s))'%v)
00349 s.write(' ((and (class %s) (= (length (%s . bv)) 1)) ;; big1'%(v,v))
00350 s.write(' (write-long (elt (%s . bv) 0) s)'%v)
00351 s.write(' (write-long (if (>= %s 0) 0 #xffffffff) s))'%v)
00352 s.write(' (t ;; integer')
00353 s.write(' (write-long %s s)(write-long (if (>= %s 0) 0 #xffffffff) s)))'%(v,v))
00354
00355
00356 def write_serialize_bits_signed(s, v, num_bytes):
00357 write_serialize_bits(s, v, num_bytes)
00358
00359 def write_serialize_builtin(s, f, v):
00360 if f.base_type == 'string':
00361 write_serialize_length(s, v)
00362 elif f.base_type == 'float32':
00363 s.write('(sys::poke %s (send s :buffer) (send s :count) :float) (incf (stream-count s) 4)'%v)
00364 elif f.base_type == 'float64':
00365 s.write('(sys::poke %s (send s :buffer) (send s :count) :double) (incf (stream-count s) 8)'%v)
00366 elif f.base_type == 'bool':
00367 s.write('(if %s (write-byte -1 s) (write-byte 0 s))'%v)
00368 elif f.base_type in ['byte', 'char']:
00369 s.write('(write-byte %s s)'%v)
00370 elif f.base_type in ['duration', 'time']:
00371 s.write('(write-long (send %s :sec) s) (write-long (send %s :nsec) s)'%(v,v))
00372 elif is_signed_int(f.base_type):
00373 write_serialize_bits_signed(s, v, NUM_BYTES[f.base_type])
00374 elif is_unsigned_int(f.base_type):
00375 write_serialize_bits(s, v, NUM_BYTES[f.base_type])
00376 else:
00377 raise ValueError('Unknown type: %s', f.base_type)
00378
00379 def write_serialize_field(s, f):
00380 s.write(';; %s _%s'%(f.type, f.name))
00381 slot = '_%s'%f.name
00382 var = slot
00383 if f.is_array and f.base_type == 'uint8':
00384 if not f.array_len:
00385 s.write('(write-long (length %s) s)'%slot)
00386 s.write('(princ %s s)'%slot)
00387 elif f.is_array and is_string(f.base_type):
00388 s.write('(write-long (length %s) s)'%slot)
00389 s.write('(dolist (elem %s)'%slot)
00390 var = 'elem'
00391 elif f.is_array:
00392 if not f.array_len:
00393 write_serialize_length(s, slot, True)
00394 if f.is_builtin and f.array_len:
00395 s.write('(dotimes (i %s)'%f.array_len)
00396 elif f.is_builtin and not f.array_len:
00397 s.write('(dotimes (i (length %s))'%var)
00398 else:
00399 s.write('(dolist (elem %s)'%slot)
00400 slot = 'elem'
00401 var = '(elt %s i)'%var
00402 s.block_next_indent()
00403 s.write('')
00404
00405 if f.is_array and f.base_type == 'uint8':
00406 pass
00407 elif f.is_builtin:
00408 with Indent(s):
00409 write_serialize_builtin(s, f, var)
00410 else:
00411 with Indent(s):
00412 s.write('(send %s :serialize s)'%slot)
00413
00414 if f.is_array and f.base_type != 'uint8':
00415 s.write(' )')
00416
00417 def write_serialize(s, spec):
00418 """
00419 Write the serialize method
00420 """
00421 with Indent(s):
00422 s.write('(:serialize')
00423 with Indent(s,inc=1):
00424 s.write('(&optional strm)')
00425 s.write('(let ((s (if strm strm')
00426 s.write(' (make-string-output-stream (send self :serialization-length)))))')
00427 with Indent(s):
00428 for f in spec.parsed_fields():
00429 write_serialize_field(s, f)
00430 s.write(';;')
00431 s.write('(if (null strm) (get-output-stream-string s))))')
00432
00433 def write_deserialize_length(s, f, v, is_array=False):
00434 if is_array:
00435 s.write('(let (n)')
00436 with Indent(s):
00437 s.write('(setq n (sys::peek buf ptr- :integer)) (incf ptr- 4)')
00438 s.write('(setq %s (let (r) (dotimes (i n) (push (instance %s :init) r)) r))'%(v,field_type(f)))
00439 else:
00440 set = 'setf' if v[0] == '(' else 'setq'
00441 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))
00442
00443 def write_deserialize_bits(s, v, num_bytes):
00444 if num_bytes == 1:
00445 type = ':char'
00446 elif num_bytes == 2:
00447 type = ':short'
00448 elif num_bytes == 4:
00449 type = ':integer'
00450 elif num_bytes == 8:
00451 type = ':long'
00452 s.write('')
00453 return write_deserialize_bits_signed(s,v,num_bytes)
00454 else:
00455 raise ValueError('Unknown size: %s', num_bytes)
00456
00457 set = 'setf' if v[0] == '(' else 'setq'
00458 s.write('(%s %s (sys::peek buf ptr- %s)) (incf ptr- %s)'%(set,v,type,num_bytes))
00459
00460 def write_deserialize_bits_signed(s, v, num_bytes):
00461 if num_bytes in [1,2,4]:
00462 write_deserialize_bits(s, v, num_bytes)
00463 else:
00464 s.write('\n', indent=False)
00465 s.write('#+(or :alpha :irix6 :x86_64)', indent=False)
00466 s.write(' (setf %s (prog1 (sys::peek buf ptr- :long) (incf ptr- 8)))\n'%v)
00467 s.write('#-(or :alpha :irix6 :x86_64)', indent=False)
00468 s.write(' (setf %s (let ((b0 (prog1 (sys::peek buf ptr- :integer) (incf ptr- 4)))'%v)
00469 s.write(' (b1 (prog1 (sys::peek buf ptr- :integer) (incf ptr- 4))))')
00470 s.write(' (cond ((= b1 -1) b0)')
00471 s.write(' ((and (= b1 0)')
00472 s.write(' (<= lisp::most-negative-fixnum b0 lisp::most-positive-fixnum))')
00473 s.write(' b0)')
00474 s.write(' ((= b1 0) (make-instance bignum :size 1 :bv (integer-vector b0)))')
00475 s.write(' (t (make-instance bignum :size 2 :bv (integer-vector b0 (ash b1 1)))))))')
00476
00477 def write_deserialize_builtin(s, f, v):
00478 set = 'setf' if v[0] == '(' else 'setq'
00479 if f.base_type == 'string':
00480 write_deserialize_length(s,f,v)
00481 elif f.base_type == 'float32':
00482 s.write('(%s %s (sys::peek buf ptr- :float)) (incf ptr- 4)'%(set, v))
00483 elif f.base_type == 'float64':
00484 s.write('(%s %s (sys::peek buf ptr- :double)) (incf ptr- 8)'%(set, v))
00485 elif f.base_type == 'bool':
00486 s.write('(%s %s (not (= 0 (sys::peek buf ptr- :char)))) (incf ptr- 1)'%(set, v))
00487 elif f.base_type in ['duration', 'time']:
00488 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))
00489 elif is_signed_int(f.base_type):
00490 write_deserialize_bits_signed(s, v, NUM_BYTES[f.base_type])
00491 if NUM_BYTES[f.base_type] == 1:
00492 s.write('(if (> %s 127) (%s %s (- %s 256)))'%(v,set,v,v))
00493 elif is_unsigned_int(f.base_type):
00494 write_deserialize_bits(s, v, NUM_BYTES[f.base_type])
00495 else:
00496 raise ValueError('%s unknown'%f.base_type)
00497
00498 def write_deserialize_field(s, f, pkg):
00499 var = '_%s'%f.name
00500 s.write(';; %s %s'%(f.type, var))
00501 if f.is_array:
00502 if f.is_builtin:
00503 if f.base_type == 'uint8':
00504 if f.array_len:
00505 s.write('(setq %s (make-array %d :element-type :char))'%(var,f.array_len))
00506 s.write('(replace %s buf :start2 ptr-) (incf ptr- %d)'%(var,f.array_len))
00507 else:
00508 s.write('(let ((n (sys::peek buf ptr- :integer))) (incf ptr- 4)')
00509 s.write(' (setq %s (make-array n :element-type :char))'%var)
00510 s.write(' (replace %s buf :start2 ptr-) (incf ptr- n))'%(var))
00511 elif f.array_len:
00512 s.write('(dotimes (i (length %s))'%var)
00513 var = '(elt %s i)'%var
00514 else:
00515 if is_float(f.base_type) or is_integer(f.base_type) or is_string(f.base_type) or is_bool(f.base_type):
00516 s.write('(let (n)')
00517 with Indent(s):
00518 s.write('(setq n (sys::peek buf ptr- :integer)) (incf ptr- 4)')
00519 if is_string(f.base_type) or is_bool(f.base_type):
00520 s.write('(setq %s (make-list n))'%var)
00521 else:
00522 s.write('(setq %s (instantiate %s-vector n))'%(var, lisp_type(f.base_type, f.is_array)))
00523 s.write('(dotimes (i n)')
00524 var = '(elt %s i)'%var
00525 else:
00526 write_deserialize_length(s, f, var, True)
00527 var = 'elem-'
00528 with Indent(s):
00529 s.write('(dolist (%s _%s)'%(var, f.name))
00530 else:
00531 if f.array_len:
00532 s.write('(dotimes (i %s)'%f.array_len)
00533 var = '(elt _%s i)'%f.name
00534 else:
00535 write_deserialize_length(s, f, var, True)
00536 var = 'elem-'
00537 with Indent(s):
00538 s.write('(dolist (%s _%s)'%(var, f.name))
00539 if f.is_array and f.base_type == 'uint8':
00540 pass
00541 elif f.is_builtin:
00542 with Indent(s):
00543 write_deserialize_builtin(s, f, var)
00544 else:
00545 with Indent(s):
00546 s.write('(send %s :deserialize buf ptr-) (incf ptr- (send %s :serialization-length))'%(var, var))
00547
00548 if f.is_array and not f.base_type == 'uint8':
00549 with Indent(s):
00550 if f.array_len:
00551 s.write(')')
00552 else:
00553 s.write('))')
00554
00555
00556 def write_deserialize(s, spec):
00557 """
00558 Write the deserialize method
00559 """
00560 with Indent(s):
00561 s.write('(:deserialize')
00562 with Indent(s,inc=1):
00563 s.write('(buf &optional (ptr- 0))')
00564 for f in spec.parsed_fields():
00565 write_deserialize_field(s, f, spec.package)
00566 s.write(';;')
00567 s.write('self)')
00568 s.write(')')
00569 s.newline()
00570
00571 def write_md5sum(s, msg_context, spec, parent=None):
00572 md5sum = genmsg.compute_md5(msg_context, parent or spec)
00573 s.write('(setf (get %s::%s :md5sum-) "%s")'%(spec.package, spec.actual_name, md5sum))
00574
00575 def write_ros_datatype(s, spec):
00576 s.write('(setf (get %s::%s :datatype-) "%s/%s")'%(spec.package, spec.actual_name, spec.package, spec.actual_name))
00577
00578 def write_message_definition(s, msg_context, spec):
00579 s.write('(setf (get %s::%s :definition-)'%(spec.package, spec.actual_name))
00580 with Indent(s,6):
00581 s.write('"')
00582 definition = genmsg.compute_full_text(msg_context, spec)
00583 lines = definition.split('\n')
00584 for line in lines:
00585 l = line.replace('\\', '\\\\')
00586 l = l.replace('"', '\\"')
00587 s.write('%s\n'%l, indent=False, newline=False)
00588 s.write('")', newline=False)
00589 s.write('\n\n')
00590
00591 def write_service_definition(s, msg_context, spec, parent):
00592 s.write('(setf (get %s::%s :definition-)'%(parent.package, parent.actual_name))
00593 with Indent(s,6):
00594 s.write('"')
00595 for spec_service in [spec.request, spec.response]:
00596 definition = genmsg.compute_full_text(msg_context, spec_service)
00597 lines = definition.split('\n')
00598 for line in lines[:-1]:
00599 l = line.replace('\\', '\\\\')
00600 l = l.replace('"', '\\"')
00601 s.write('%s\n'%l, indent=False, newline=False)
00602 if spec_service == spec.request:
00603 s.write('---\n', indent=False, newline=False)
00604 s.write('")', newline=False)
00605
00606 def write_builtin_length(s, f, var='msg'):
00607 if f.base_type in ['int8', 'uint8']:
00608 s.write('1')
00609 elif f.base_type in ['int16', 'uint16']:
00610 s.write('2')
00611 elif f.base_type in ['int32', 'uint32', 'float32']:
00612 s.write('4')
00613 elif f.base_type in ['int64', 'uint64', 'float64', 'duration', 'time']:
00614 s.write('8')
00615 elif f.base_type == 'string':
00616 s.write('4 (length _%s)'%f.name)
00617 elif f.base_type in ['bool', 'byte', 'char']:
00618 s.write('1')
00619 else:
00620 raise ValueError('Unknown: %s', f.base_type)
00621
00622 def write_serialization_length(s, spec):
00623 with Indent(s):
00624 s.write('(:serialization-length')
00625 with Indent(s, inc=1):
00626 s.write('()')
00627 s.write('(+')
00628 with Indent(s, 1):
00629 if not spec.parsed_fields():
00630 s.write('0')
00631 for field in spec.parsed_fields():
00632 s.write(';; %s _%s'%(field.type, field.name))
00633 if field.is_array:
00634 if field.is_builtin and not is_string(field.base_type):
00635 s.write('(* ')
00636 else:
00637 s.write('(apply #\'+ ')
00638 s.block_next_indent()
00639
00640 if field.is_builtin:
00641 if not field.array_len:
00642 if is_string(field.base_type):
00643 s.write('(mapcar #\'(lambda (x) (+ 4 (length x))) _%s)) 4'%(field.name))
00644 else:
00645 write_builtin_length(s, field)
00646 s.write('(length _%s)) 4'%field.name, newline=False)
00647 else:
00648 write_builtin_length(s, field)
00649 s.write('%s)'%field.array_len, newline=False)
00650 else:
00651 if field.array_len:
00652 s.write('(send-all _%s :serialization-length))'%field.name)
00653 else:
00654 s.write('(send-all _%s :serialization-length)) 4'%field.name)
00655 else:
00656 if field.is_builtin:
00657 write_builtin_length(s, field)
00658 else:
00659 s.write('(send _%s :serialization-length)'%field.name)
00660
00661 s.write('))')
00662
00663
00664 def write_provide(s, msg_context, spec):
00665 md5sum = genmsg.compute_md5(msg_context, spec)
00666 s.write('(provide :%s/%s "%s")'%(spec.package, spec.actual_name,md5sum))
00667 s.write('\n')
00668
00669 def write_constants(s, spec):
00670 if spec.constants:
00671 for c in spec.constants:
00672 s.write('(intern "*%s*" (find-package "%s::%s"))'%(c.name.upper(), spec.package.upper(), spec.actual_name.upper()))
00673 s.write('(shadow \'*%s* (find-package "%s::%s"))'%(c.name.upper(), spec.package.upper(), spec.actual_name.upper()))
00674 if c.type == 'string':
00675 s.write('(defconstant %s::%s::*%s* "%s")'%(spec.package, spec.actual_name, c.name.upper(), c.val.replace('"', '\\"')))
00676 elif c.type == 'bool':
00677 s.write('(defconstant %s::%s::*%s* %s)'%(spec.package, spec.actual_name, c.name.upper(), "t" if c.val == True else "nil"))
00678 else:
00679 s.write('(defconstant %s::%s::*%s* %s)'%(spec.package, spec.actual_name, c.name.upper(), c.val))
00680
00681 def write_srv_component(s, spec, context, parent):
00682 spec.component_type='service'
00683 write_constants(s, spec)
00684 write_defclass(s, spec)
00685 write_defmethod(s, spec)
00686 write_accessors(s, spec)
00687 write_serialization_length(s, spec)
00688 write_serialize(s, spec)
00689 write_deserialize(s, spec)
00690
00691 def write_service_specific_methods(s, context, spec):
00692
00693 s.write('(defclass %s::%s'%(spec.package, spec.actual_name))
00694 with Indent(s):
00695 s.write(':super ros::object')
00696 s.write(':slots ())')
00697 s.newline()
00698 write_md5sum(s, context, spec, parent=spec)
00699 write_ros_datatype(s, spec)
00700 s.write('(setf (get %s::%s :request) %s::%s)'%(spec.package, spec.actual_name, spec.request.package, spec.request.actual_name))
00701 s.write('(setf (get %s::%s :response) %s::%s)'%(spec.package, spec.actual_name, spec.response.package, spec.response.actual_name))
00702 s.newline()
00703 s.write('(defmethod %s::%s'%(spec.request.package, spec.request.actual_name))
00704 s.write(' (:response () (instance %s::%s :init)))'%(spec.response.package, spec.response.actual_name))
00705 s.newline()
00706 for spec_service in [spec.request, spec.response]:
00707 write_md5sum(s, context, spec_service, parent=spec)
00708 write_ros_datatype(s, spec_service)
00709 write_service_definition(s, context, spec, spec_service)
00710 s.newline()
00711 s.write('\n')
00712 write_provide(s, context, spec)
00713 s.write('\n', newline=False)
00714
00715 def generate_msg(pkg, files, out_dir, search_path):
00716 """
00717 Generate euslisp code for all messages in a package
00718 """
00719 msg_context = MsgContext.create_default()
00720 for f in files:
00721 f = os.path.abspath(f)
00722 infile = os.path.basename(f)
00723 full_type = genmsg.gentools.compute_full_type_name(pkg, infile)
00724 spec = genmsg.msg_loader.load_msg_from_file(msg_context, f, full_type)
00725 generate_msg_from_spec(msg_context, spec, search_path, out_dir, pkg)
00726
00727 def generate_srv(pkg, files, out_dir, search_path):
00728 """
00729 Generate euslisp code for all services in a package
00730 """
00731 msg_context = MsgContext.create_default()
00732 for f in files:
00733 f = os.path.abspath(f)
00734 infile = os.path.basename(f)
00735 full_type = genmsg.gentools.compute_full_type_name(pkg, infile)
00736 spec = genmsg.msg_loader.load_srv_from_file(msg_context, f, full_type)
00737 generate_srv_from_spec(msg_context, spec, search_path, out_dir, pkg, f)
00738
00739 def msg_list(pkg, search_path, ext):
00740 dir_list = search_path[pkg]
00741 files = []
00742 for d in dir_list:
00743 files.extend([f for f in os.listdir(d) if f.endswith(ext)])
00744 return [f[:-len(ext)] for f in files]
00745
00746 def generate_msg_from_spec(msg_context, spec, search_path, output_dir, package):
00747 """
00748 Generate a message
00749
00750 @param msg_path: The path to the .msg file
00751 @type msg_path: str
00752 """
00753 genmsg.msg_loader.load_depends(msg_context, spec, search_path)
00754 spec.actual_name=spec.short_name
00755 spec.component_type='message'
00756 msgs = msg_list(package, search_path, '.msg')
00757 for m in msgs:
00758 genmsg.load_msg_by_type(msg_context, '%s/%s'%(package, m), search_path)
00759
00760
00761
00762
00763
00764
00765 io = StringIO()
00766 s = IndentedWriter(io)
00767 write_begin(s, spec)
00768 write_include(s, spec)
00769 write_constants(s, spec)
00770 write_defclass(s, spec)
00771 write_defmethod(s, spec)
00772 write_accessors(s, spec)
00773 write_serialization_length(s, spec)
00774 write_serialize(s, spec)
00775 write_deserialize(s, spec)
00776 write_md5sum(s, msg_context, spec)
00777 write_ros_datatype(s, spec)
00778 write_message_definition(s, msg_context, spec)
00779 write_provide(s, msg_context, spec)
00780
00781 if (not os.path.exists(output_dir)):
00782
00783
00784 try:
00785 os.makedirs(output_dir)
00786 except OSError as e:
00787 pass
00788
00789 with open('%s/%s.l'%(output_dir, spec.short_name), 'w') as f:
00790 f.write(io.getvalue() + "\n")
00791 io.close()
00792
00793
00794
00795 def generate_srv_from_spec(msg_context, spec, search_path, output_dir, package, path):
00796 "Generate code from .srv file"
00797 genmsg.msg_loader.load_depends(msg_context, spec, search_path)
00798 ext = '.srv'
00799 srvs = [f[:-len(ext)] for f in os.listdir(os.path.dirname(path)) if f.endswith(ext)]
00800 for s in srvs:
00801 load_srv_from_file(msg_context, path, '%s/%s'%(package, s))
00802
00803
00804
00805
00806
00807 io = StringIO()
00808 s = IndentedWriter(io)
00809 write_begin(s, spec, True)
00810 write_include(s, spec.request, is_srv=True)
00811 write_include(s, spec.response, is_srv=True)
00812 spec.request.actual_name='%sRequest'%spec.short_name
00813 spec.response.actual_name='%sResponse'%spec.short_name
00814 write_srv_component(s, spec.request, msg_context, spec)
00815 write_srv_component(s, spec.response, msg_context, spec)
00816 write_service_specific_methods(s, msg_context, spec)
00817
00818 with open('%s/%s.l'%(output_dir, spec.short_name), 'w') as f:
00819 f.write(io.getvalue())
00820 io.close()
00821
00822