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 __author__ = "mferguson@willowgarage.com (Michael Ferguson)"
00037
00038 __usage__ = """
00039 make_library.py generates the Arduino rosserial library files. It
00040 requires the location of your arduino libraries folder and the name of
00041 one or more packages for which you want to make libraries.
00042
00043 rosrun rosserial_client make_library.py <output_path> pkg_name [pkg2 pkg3 ...]
00044 """
00045
00046 import roslib; roslib.load_manifest("rosserial_client")
00047 import roslib.gentools, roslib.srvs
00048 import roslib.rospack
00049 import rospy
00050
00051 import os, sys, subprocess, re
00052
00053 def type_to_var(ty):
00054 lookup = {
00055 1 : 'uint8_t',
00056 2 : 'uint16_t',
00057 4 : 'uint32_t',
00058 8 : 'uint64_t',
00059 }
00060 return lookup[ty]
00061
00062
00063
00064
00065 class EnumerationType:
00066 """ For data values. """
00067
00068 def __init__(self, name, ty, value):
00069 self.name = name
00070 self.type = ty
00071 self.value = value
00072
00073 def make_declaration(self, f):
00074 f.write(' enum { %s = %s };\n' % (self.name, self.value))
00075
00076 class PrimitiveDataType:
00077 """ Our datatype is a C/C++ primitive. """
00078
00079 def __init__(self, name, ty, bytes):
00080 self.name = name
00081 self.type = ty
00082 self.bytes = bytes
00083
00084 def make_declaration(self, f):
00085 f.write(' %s %s;\n' % (self.type, self.name) )
00086
00087 def serialize(self, f):
00088 cn = self.name.replace("[","").replace("]","").split(".")[-1]
00089 if self.type != type_to_var(self.bytes):
00090 f.write(' union {\n')
00091 f.write(' %s real;\n' % self.type)
00092 f.write(' %s base;\n' % type_to_var(self.bytes))
00093 f.write(' } u_%s;\n' % cn)
00094 f.write(' u_%s.real = this->%s;\n' % (cn,self.name))
00095 for i in range(self.bytes):
00096 f.write(' *(outbuffer + offset + %d) = (u_%s.base >> (8 * %d)) & 0xFF;\n' % (i, cn, i) )
00097 else:
00098 for i in range(self.bytes):
00099 f.write(' *(outbuffer + offset + %d) = (this->%s >> (8 * %d)) & 0xFF;\n' % (i, self.name, i) )
00100 f.write(' offset += sizeof(this->%s);\n' % self.name)
00101
00102 def deserialize(self, f):
00103 cn = self.name.replace("[","").replace("]","").split(".")[-1]
00104 if self.type != type_to_var(self.bytes):
00105 f.write(' union {\n')
00106 f.write(' %s real;\n' % self.type)
00107 f.write(' %s base;\n' % type_to_var(self.bytes))
00108 f.write(' } u_%s;\n' % cn)
00109 f.write(' u_%s.base = 0;\n' % cn)
00110 for i in range(self.bytes):
00111 f.write(' u_%s.base |= ((%s) (*(inbuffer + offset + %d))) << (8 * %d);\n' % (cn,type_to_var(self.bytes),i,i) )
00112 f.write(' this->%s = u_%s.real;\n' % (self.name, cn) )
00113 else:
00114 for i in range(self.bytes):
00115 f.write(' this->%s |= ((%s) (*(inbuffer + offset + %d))) << (8 * %d);\n' % (self.name,self.type,i,i) )
00116 f.write(' offset += sizeof(this->%s);\n' % self.name)
00117
00118
00119 class MessageDataType(PrimitiveDataType):
00120 """ For when our data type is another message. """
00121 def serialize(self, f):
00122 f.write(' offset += this->%s.serialize(outbuffer + offset);\n' % self.name)
00123
00124 def deserialize(self, f):
00125 f.write(' offset += this->%s.deserialize(inbuffer + offset);\n' % self.name)
00126
00127
00128 class Float64DataType(PrimitiveDataType):
00129 """ AVR C/C++ has no native 64-bit support, we automatically convert to 32-bit float. """
00130
00131 def make_declaration(self, f):
00132 f.write(' float %s;\n' % self.name )
00133
00134 def serialize(self, f):
00135 cn = self.name.replace("[","").replace("]","")
00136 f.write(' int32_t * val_%s = (long *) &(this->%s);\n' % (cn,self.name))
00137 f.write(' int32_t exp_%s = (((*val_%s)>>23)&255);\n' % (cn,cn))
00138 f.write(' if(exp_%s != 0)\n' % cn)
00139 f.write(' exp_%s += 1023-127;\n' % cn)
00140 f.write(' int32_t sig_%s = *val_%s;\n' % (cn,cn))
00141 f.write(' *(outbuffer + offset++) = 0;\n')
00142 f.write(' *(outbuffer + offset++) = 0;\n')
00143 f.write(' *(outbuffer + offset++) = 0;\n')
00144 f.write(' *(outbuffer + offset++) = (sig_%s<<5) & 0xff;\n' % cn)
00145 f.write(' *(outbuffer + offset++) = (sig_%s>>3) & 0xff;\n' % cn)
00146 f.write(' *(outbuffer + offset++) = (sig_%s>>11) & 0xff;\n' % cn)
00147 f.write(' *(outbuffer + offset++) = ((exp_%s<<4) & 0xF0) | ((sig_%s>>19)&0x0F);\n' % (cn,cn))
00148 f.write(' *(outbuffer + offset++) = (exp_%s>>4) & 0x7F;\n' % cn)
00149 f.write(' if(this->%s < 0) *(outbuffer + offset -1) |= 0x80;\n' % self.name)
00150
00151 def deserialize(self, f):
00152 cn = self.name.replace("[","").replace("]","")
00153 f.write(' uint32_t * val_%s = (uint32_t*) &(this->%s);\n' % (cn,self.name))
00154 f.write(' offset += 3;\n')
00155 f.write(' *val_%s = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);\n' % cn)
00156 f.write(' *val_%s |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;\n' % cn)
00157 f.write(' *val_%s |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;\n' % cn)
00158 f.write(' *val_%s |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;\n' % cn)
00159 f.write(' uint32_t exp_%s = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;\n' % cn)
00160 f.write(' exp_%s |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;\n' % cn)
00161 f.write(' if(exp_%s !=0)\n' % cn)
00162 f.write(' *val_%s |= ((exp_%s)-1023+127)<<23;\n' % (cn,cn))
00163 f.write(' if( ((*(inbuffer+offset++)) & 0x80) > 0) this->%s = -this->%s;\n' % (self.name,self.name))
00164
00165
00166 class StringDataType(PrimitiveDataType):
00167 """ Need to convert to signed char *. """
00168
00169 def make_declaration(self, f):
00170 f.write(' char * %s;\n' % self.name)
00171
00172 def serialize(self, f):
00173 cn = self.name.replace("[","").replace("]","")
00174 f.write(' uint32_t * length_%s = (uint32_t *)(outbuffer + offset);\n' % cn)
00175 f.write(' *length_%s = strlen( (const char*) this->%s);\n' % (cn,self.name))
00176 f.write(' offset += 4;\n')
00177 f.write(' memcpy(outbuffer + offset, this->%s, *length_%s);\n' % (self.name,cn))
00178 f.write(' offset += *length_%s;\n' % cn)
00179
00180 def deserialize(self, f):
00181 cn = self.name.replace("[","").replace("]","")
00182 f.write(' uint32_t length_%s = *(uint32_t *)(inbuffer + offset);\n' % cn)
00183 f.write(' offset += 4;\n')
00184 f.write(' for(unsigned int k= offset; k< offset+length_%s; ++k){\n'%cn)
00185 f.write(' inbuffer[k-1]=inbuffer[k];\n')
00186 f.write(' }\n')
00187 f.write(' inbuffer[offset+length_%s-1]=0;\n'%cn)
00188 f.write(' this->%s = (char *)(inbuffer + offset-1);\n' % self.name)
00189 f.write(' offset += length_%s;\n' % cn)
00190
00191
00192 class TimeDataType(PrimitiveDataType):
00193
00194 def __init__(self, name, ty, bytes):
00195 self.name = name
00196 self.type = ty
00197 self.sec = PrimitiveDataType(name+'.sec','uint32_t',4)
00198 self.nsec = PrimitiveDataType(name+'.nsec','uint32_t',4)
00199
00200 def make_declaration(self, f):
00201 f.write(' %s %s;\n' % (self.type, self.name))
00202
00203 def serialize(self, f):
00204 self.sec.serialize(f)
00205 self.nsec.serialize(f)
00206
00207 def deserialize(self, f):
00208 self.sec.deserialize(f)
00209 self.nsec.deserialize(f)
00210
00211
00212 class ArrayDataType(PrimitiveDataType):
00213
00214 def __init__(self, name, ty, bytes, cls, array_size=None):
00215 self.name = name
00216 self.type = ty
00217 self.bytes = bytes
00218 self.size = array_size
00219 self.cls = cls
00220
00221 def make_declaration(self, f):
00222 c = self.cls("*"+self.name, self.type, self.bytes)
00223 if self.size == None:
00224 f.write(' uint8_t %s_length;\n' % self.name)
00225 f.write(' %s st_%s;\n' % (self.type, self.name))
00226 f.write(' %s * %s;\n' % (self.type, self.name))
00227 else:
00228 f.write(' %s %s[%d];\n' % (self.type, self.name, self.size))
00229
00230 def serialize(self, f):
00231 c = self.cls(self.name+"[i]", self.type, self.bytes)
00232 if self.size == None:
00233
00234 f.write(' *(outbuffer + offset++) = %s_length;\n' % self.name)
00235 f.write(' *(outbuffer + offset++) = 0;\n')
00236 f.write(' *(outbuffer + offset++) = 0;\n')
00237 f.write(' *(outbuffer + offset++) = 0;\n')
00238 f.write(' for( uint8_t i = 0; i < %s_length; i++){\n' % self.name)
00239 c.serialize(f)
00240 f.write(' }\n')
00241 else:
00242 f.write(' unsigned char * %s_val = (unsigned char *) this->%s;\n' % (self.name, self.name))
00243 f.write(' for( uint8_t i = 0; i < %d; i++){\n' % (self.size) )
00244 c.serialize(f)
00245 f.write(' }\n')
00246
00247 def deserialize(self, f):
00248 if self.size == None:
00249 c = self.cls("st_"+self.name, self.type, self.bytes)
00250
00251 f.write(' uint8_t %s_lengthT = *(inbuffer + offset++);\n' % self.name)
00252 f.write(' if(%s_lengthT > %s_length)\n' % (self.name, self.name))
00253 f.write(' this->%s = (%s*)realloc(this->%s, %s_lengthT * sizeof(%s));\n' % (self.name, self.type, self.name, self.name, self.type))
00254 f.write(' offset += 3;\n')
00255 f.write(' %s_length = %s_lengthT;\n' % (self.name, self.name))
00256
00257 f.write(' for( uint8_t i = 0; i < %s_length; i++){\n' % (self.name) )
00258 c.deserialize(f)
00259 f.write(' memcpy( &(this->%s[i]), &(this->st_%s), sizeof(%s));\n' % (self.name, self.name, self.type))
00260 f.write(' }\n')
00261 else:
00262 c = self.cls(self.name+"[i]", self.type, self.bytes)
00263 f.write(' uint8_t * %s_val = (uint8_t*) this->%s;\n' % (self.name, self.name))
00264 f.write(' for( uint8_t i = 0; i < %d; i++){\n' % (self.size) )
00265 c.deserialize(f)
00266 f.write(' }\n')
00267
00268
00269 ros_to_arduino_types = {
00270 'bool' : ('bool', 1, PrimitiveDataType, []),
00271 'int8' : ('int8_t', 1, PrimitiveDataType, []),
00272 'uint8' : ('uint8_t', 1, PrimitiveDataType, []),
00273 'int16' : ('int16_t', 2, PrimitiveDataType, []),
00274 'uint16' : ('uint16_t', 2, PrimitiveDataType, []),
00275 'int32' : ('int32_t', 4, PrimitiveDataType, []),
00276 'uint32' : ('uint32_t', 4, PrimitiveDataType, []),
00277 'int64' : ('int64_t', 4, PrimitiveDataType, []),
00278 'uint64' : ('uint64_t', 4, PrimitiveDataType, []),
00279 'float32' : ('float', 4, PrimitiveDataType, []),
00280 'float64' : ('float', 4, Float64DataType, []),
00281 'time' : ('ros::Time', 8, TimeDataType, ['ros/time']),
00282 'duration': ('ros::Duration', 8, TimeDataType, ['ros/duration']),
00283 'string' : ('char*', 0, StringDataType, []),
00284 'Header' : ('std_msgs::Header', 0, MessageDataType, ['std_msgs/Header'])
00285 }
00286
00287
00288
00289
00290
00291 class Message:
00292 """ Parses message definitions into something we can export. """
00293
00294 def __init__(self, name, package, definition, md5):
00295
00296 self.name = name
00297 self.package = package
00298 self.md5 = md5
00299 self.includes = list()
00300
00301 self.data = list()
00302 self.enums = list()
00303
00304
00305 for line in definition:
00306
00307 line = line.strip().rstrip()
00308 value = None
00309 if line.find("#") > -1:
00310 line = line[0:line.find("#")]
00311 if line.find("=") > -1:
00312 try:
00313 value = line[line.find("=")+1:]
00314 except:
00315 value = '"' + line[line.find("=")+1:] + '"';
00316 line = line[0:line.find("=")]
00317
00318
00319 line = line.replace("\t", " ")
00320 l = line.split(" ")
00321 while "" in l:
00322 l.remove("")
00323 if len(l) < 2:
00324 continue
00325 ty, name = l[0:2]
00326 if value != None:
00327 self.enums.append( EnumerationType(name, ty, value))
00328 continue
00329
00330 try:
00331 type_package, type_name = ty.split("/")
00332 except:
00333 type_package = None
00334 type_name = ty
00335 type_array = False
00336 if type_name.find('[') > 0:
00337 type_array = True
00338 try:
00339 type_array_size = int(type_name[type_name.find('[')+1:type_name.find(']')])
00340 except:
00341 type_array_size = None
00342 type_name = type_name[0:type_name.find('[')]
00343
00344
00345 try:
00346 code_type = ros_to_arduino_types[type_name][0]
00347 size = ros_to_arduino_types[type_name][1]
00348 cls = ros_to_arduino_types[type_name][2]
00349 for include in ros_to_arduino_types[type_name][3]:
00350 if include not in self.includes:
00351 self.includes.append(include)
00352 except:
00353 if type_package == None:
00354 type_package = self.package
00355 if type_package+"/"+type_name not in self.includes:
00356 self.includes.append(type_package+"/"+type_name)
00357 cls = MessageDataType
00358 code_type = type_package + "::" + type_name
00359 size = 0
00360 if type_array:
00361 self.data.append( ArrayDataType(name, code_type, size, cls, type_array_size ) )
00362 else:
00363 self.data.append( cls(name, code_type, size) )
00364
00365 def _write_serializer(self, f):
00366
00367 f.write('\n')
00368 f.write(' virtual int serialize(unsigned char *outbuffer) const\n')
00369 f.write(' {\n')
00370 f.write(' int offset = 0;\n')
00371 for d in self.data:
00372 d.serialize(f)
00373 f.write(' return offset;\n');
00374 f.write(' }\n')
00375 f.write('\n')
00376
00377 def _write_deserializer(self, f):
00378
00379 f.write(' virtual int deserialize(unsigned char *inbuffer)\n')
00380 f.write(' {\n')
00381 f.write(' int offset = 0;\n')
00382 for d in self.data:
00383 d.deserialize(f)
00384 f.write(' return offset;\n');
00385 f.write(' }\n')
00386 f.write('\n')
00387
00388 def _write_std_includes(self, f):
00389 f.write('#include <stdint.h>\n')
00390 f.write('#include <string.h>\n')
00391 f.write('#include <stdlib.h>\n')
00392 f.write('#include "ros/msg.h"\n')
00393
00394 def _write_msg_includes(self,f):
00395 for i in self.includes:
00396 f.write('#include "%s.h"\n' % i)
00397
00398 def _write_data(self, f):
00399 for d in self.data:
00400 d.make_declaration(f)
00401 for e in self.enums:
00402 e.make_declaration(f)
00403
00404 def _write_getType(self, f):
00405 f.write(' const char * getType(){ return "%s/%s"; };\n'%(self.package, self.name))
00406
00407 def _write_getMD5(self, f):
00408 f.write(' const char * getMD5(){ return "%s"; };\n'%self.md5)
00409
00410 def _write_impl(self, f):
00411 f.write(' class %s : public ros::Msg\n' % self.name)
00412 f.write(' {\n')
00413 f.write(' public:\n')
00414 self._write_data(f)
00415 self._write_serializer(f)
00416 self._write_deserializer(f)
00417 self._write_getType(f)
00418 self._write_getMD5(f)
00419 f.write('\n')
00420 f.write(' };\n')
00421
00422 def make_header(self, f):
00423 f.write('#ifndef _ROS_%s_%s_h\n'%(self.package, self.name))
00424 f.write('#define _ROS_%s_%s_h\n'%(self.package, self.name))
00425 f.write('\n')
00426 self._write_std_includes(f)
00427 self._write_msg_includes(f)
00428
00429 f.write('\n')
00430 f.write('namespace %s\n' % self.package)
00431 f.write('{\n')
00432 f.write('\n')
00433 self._write_impl(f)
00434 f.write('\n')
00435 f.write('}\n')
00436
00437 f.write('#endif')
00438
00439 class Service:
00440 def __init__(self, name, package, definition, md5req, md5res):
00441 """
00442 @param name - name of service
00443 @param package - name of service package
00444 @param definition - list of lines of definition
00445 """
00446
00447 self.name = name
00448 self.package = package
00449
00450 sep_line = None
00451 sep = re.compile('---*')
00452 for i in range(0, len(definition)):
00453 if (None!= re.match(sep, definition[i]) ):
00454 sep_line = i
00455 break
00456 self.req_def = definition[0:sep_line]
00457 self.resp_def = definition[sep_line+1:]
00458
00459 self.req = Message(name+"Request", package, self.req_def, md5req)
00460 self.resp = Message(name+"Response", package, self.resp_def, md5res)
00461
00462 def make_header(self, f):
00463 f.write('#ifndef _ROS_SERVICE_%s_h\n' % self.name)
00464 f.write('#define _ROS_SERVICE_%s_h\n' % self.name)
00465
00466 self.req._write_std_includes(f)
00467 includes = self.req.includes
00468 includes.extend(self.resp.includes)
00469 includes = list(set(includes))
00470 for inc in includes:
00471 f.write('#include "%s.h"\n' % inc)
00472
00473 f.write('\n')
00474 f.write('namespace %s\n' % self.package)
00475 f.write('{\n')
00476 f.write('\n')
00477 f.write('static const char %s[] = "%s/%s";\n'%(self.name.upper(), self.package, self.name))
00478
00479 def write_type(out, name):
00480 out.write(' const char * getType(){ return %s; };\n'%(name))
00481 _write_getType = lambda out: write_type(out, self.name.upper())
00482 self.req._write_getType = _write_getType
00483 self.resp._write_getType = _write_getType
00484
00485 f.write('\n')
00486 self.req._write_impl(f)
00487 f.write('\n')
00488 self.resp._write_impl(f)
00489 f.write('\n')
00490 f.write(' class %s {\n' % self.name )
00491 f.write(' public:\n')
00492 f.write(' typedef %s Request;\n' % self.req.name )
00493 f.write(' typedef %s Response;\n' % self.resp.name )
00494 f.write(' };\n')
00495 f.write('\n')
00496
00497 f.write('}\n')
00498
00499 f.write('#endif')
00500
00501
00502
00503
00504
00505
00506 def MakeLibrary(package, output_path):
00507 print "Exporting " + package + "\n",
00508
00509 pkg_dir = roslib.packages.get_pkg_dir(package)
00510
00511 sys.stdout.write(' Messages:')
00512
00513 messages = list()
00514 if os.path.exists(pkg_dir+"/msg"):
00515 sys.stdout.write('\n ')
00516 for f in os.listdir(pkg_dir+"/msg"):
00517 if f.endswith(".msg"):
00518 file = pkg_dir + "/msg/" + f
00519
00520 print "%s," % f[0:-4],
00521 definition = open(file).readlines()
00522 md5sum = roslib.gentools.compute_md5(roslib.gentools.get_file_dependencies(file))
00523 messages.append( Message(f[0:-4], package, definition, md5sum) )
00524 print "\n"
00525
00526 sys.stdout.write(' Services:')
00527
00528 services = list()
00529 if (os.path.exists(pkg_dir+"/srv/")):
00530 sys.stdout.write('\n ')
00531 for f in os.listdir(pkg_dir+"/srv"):
00532 if f.endswith(".srv"):
00533 file = pkg_dir + "/srv/" + f
00534
00535 print "%s," % f[0:-4],
00536 definition, service = roslib.srvs.load_from_file(file)
00537 definition = open(file).readlines()
00538 md5req = roslib.gentools.compute_md5(roslib.gentools.get_dependencies(service.request, package))
00539 md5res = roslib.gentools.compute_md5(roslib.gentools.get_dependencies(service.response, package))
00540 messages.append( Service(f[0:-4], package, definition, md5req, md5res ) )
00541 print "\n"
00542
00543
00544 output_path = output_path + "/" + package
00545 for msg in messages:
00546 if not os.path.exists(output_path):
00547 os.makedirs(output_path)
00548 header = open(output_path + "/" + msg.name + ".h", "w")
00549 msg.make_header(header)
00550 header.close()
00551
00552
00553 def add_depends(packages, package):
00554 depend = [package] + roslib.rospack.rospack_depends(package)
00555 for p in depend:
00556 if not p in packages:
00557 packages.append(p)
00558 packages = add_depends(packages, p)
00559 return packages
00560
00561 if __name__=="__main__":
00562
00563
00564 if (len(sys.argv) <3):
00565 print __usage__
00566 exit()
00567
00568
00569 path = sys.argv[1]
00570 if path[-1] == "/":
00571 path = path[0:-1]
00572 path += "/ros_lib"
00573 print "\nExporting to %s" % path
00574
00575 packages = list()
00576
00577 for package in sys.argv[2:]:
00578 packages = add_depends(packages, package)
00579
00580 print packages
00581 for package in packages:
00582 MakeLibrary(package, path)
00583