00001
00002
00003 from optparse import OptionParser
00004 import os, os.path, sys, string, re
00005
00006
00007
00008 sppath = "/usr/lib/omniidl"
00009 if os.path.isdir(sppath):
00010 sys.path.append(sppath)
00011
00012 from omniidl import idlast, idlvisitor, idlutil, idltype
00013 from omniidl_be import cxx
00014 import _omniidl
00015
00016
00017
00018
00019
00020 TypeNameMap = {
00021 idltype.tk_boolean: 'bool',
00022 idltype.tk_char: 'int8',
00023 idltype.tk_octet: 'uint8',
00024 idltype.tk_wchar: 'int16',
00025 idltype.tk_short: 'int16',
00026 idltype.tk_ushort: 'uint16',
00027 idltype.tk_long: 'int32',
00028 idltype.tk_ulong: 'uint32',
00029 idltype.tk_longlong: 'int64',
00030 idltype.tk_ulonglong: 'uint64',
00031 idltype.tk_float: 'float32',
00032 idltype.tk_double: 'float64',
00033 idltype.tk_string: 'string',
00034 idltype.tk_wstring: 'string',
00035 idltype.tk_any: 'string',
00036 idltype.tk_TypeCode: 'uint64',
00037 idltype.tk_enum: 'uint64'}
00038
00039 MultiArrayTypeNameMap = {
00040 idltype.tk_char: 'std_msgs/Int8MultiArray',
00041 idltype.tk_octet: 'std_msgs/Uint8MultiArray',
00042 idltype.tk_wchar: 'std_msgs/Int16MultiArray',
00043 idltype.tk_short: 'std_msgs/Int16MultiArray',
00044 idltype.tk_ushort: 'std_msgs/Uint16MultiArray',
00045 idltype.tk_long: 'std_msgs/Int32MultiArray',
00046 idltype.tk_ulong: 'std_msgs/Uint32MultiArray',
00047 idltype.tk_longlong: 'std_msgs/Int64MultiArray',
00048 idltype.tk_ulonglong:'std_msgs/Uint64MultiArray',
00049 idltype.tk_float: 'std_msgs/Float32MultiArray',
00050 idltype.tk_double: 'std_msgs/Float64MultiArray',
00051 idltype.tk_string: 'openrtm_ros_bridge/StringMultiArray'}
00052
00053
00054
00055 convert_functions = """\n
00056 template<typename T,typename S>
00057 void genseq(S& s, int size, _CORBA_Unbounded_Sequence<T>* p){
00058 s = S(size,size,S::allocbuf(size), 1);}
00059 template<typename T,typename S,int n>
00060 void genseq(S& s, int size, _CORBA_Bounded_Sequence<T,n>* p){
00061 s = S(size,S::allocbuf(size), 1);}
00062 template<class T1, class T2> inline
00063 void convert(T1& in, T2& out){ out = static_cast<T2>(in); }
00064 template<typename T>
00065 void convert(T& in, std::string& out){ out = std::string(in); }
00066 template<typename T>
00067 void convert(std::string& in, T& out){ out = static_cast<T>(in.c_str()); }
00068 void convert(_CORBA_String_element in, std::string& out){ out = std::string(in); }
00069 void convert(std::string& in, _CORBA_String_element out){ out = (const char*)in.c_str(); }
00070 template<class S,class T>
00071 void convert(S& s, std::vector<T>& v){
00072 int size = s.length();
00073 v = std::vector<T>(s.length());
00074 for(int i=0; i<size; i++) convert(s[i],v[i]);}
00075 template<class S,class T>
00076 void convert(std::vector<T>& v, S& s){
00077 int size = v.size();
00078 s = S(size, size, S::allocbuf(size), 1);
00079 for(int i=0; i<size; i++) convert(v[i],s[i]);}
00080 template<class S,class T,std::size_t n>
00081 void convert(S& s, boost::array<T,n>& v){
00082 for(std::size_t i=0; i<n; i++) convert(s[i],v[i]);}
00083 template<class S,class T,std::size_t n>
00084 void convert(boost::array<T,n>& v, S& s){
00085 s = S(n, S::allocbuf(n), 1);
00086 for(std::size_t i=0; i<n; i++) convert(v[i],s[i]);}
00087 template<typename S,class T,std::size_t n>
00088 void convert(boost::array<T,n>& v, S (&s)[n]){
00089 for(std::size_t i=0; i<n; i++) convert(v[i],s[i]);}
00090
00091 // special case for RTC::LightweightRTObject_var
00092 template<class T> void convert(T& in, RTC::LightweightRTObject_var out){ std::cerr << "convert from RTC::LightweightRTObject_var is not supported" << std::endl; }
00093 """
00094
00095 multiarray_conversion = """
00096 template<class S> // convert multi-dimensional array
00097 void convert(S& s, %s& v){
00098 if(v.layout.dim.size()==0 || v.layout.dim[v.layout.dim.size()-1].size==0) {
00099 int level = v.layout.dim.size();
00100 v.layout.dim.push_back(std_msgs::MultiArrayDimension());
00101 v.layout.dim[level].stride = 1;
00102 for(uint i=0; i<s.length(); i++){
00103 convert(s[i], v);
00104 if(i==0) v.layout.dim[level].size = s.length(); // initialized flag
00105 }
00106 for(uint i=level; i<v.layout.dim.size(); i++)
00107 v.layout.dim[level].stride *= v.layout.dim[level].size;
00108 } else {
00109 for(uint i=0; i<s.length(); i++){ convert(s[i], v); }
00110 }
00111 }
00112 template<>
00113 void convert(%s& s, %s& v){ v.data.push_back(s); }
00114 template<class S>
00115 void convert(%s& v, S& s){
00116 int level, size;
00117 for(level=0; (size=v.layout.dim[level].size)==0; level++);
00118 genseq(s, size, &s);
00119 v.layout.dim[level].size = 0;
00120 for(uint i=0; i<s.length(); i++)
00121 convert(v, s[i]);
00122 v.layout.dim[level].size = size;
00123 }
00124 template<>
00125 void convert(%s& v, %s& s){ convert(v.data[v.layout.data_offset++], s); }
00126 """
00127
00128
00129 NOT_FULL = 0
00130 ROS_FULL = 1
00131 CPP_FULL = 2
00132
00133
00134 class ServiceVisitor (idlvisitor.AstVisitor):
00135
00136 def __init__(self):
00137 self.generated_msgs = []
00138
00139 def visitAST(self, node):
00140 for n in node.declarations():
00141 n.accept(self)
00142 def visitModule(self, node):
00143 for n in node.definitions():
00144 n.accept(self)
00145 def visitInterface(self, node):
00146 self.outputMsg(node)
00147 for c in node.contents():
00148 c.accept(self)
00149 if node.mainFile():
00150 self.genBridgeComponent(node)
00151
00152 def visitOperation(self, node):
00153 if node.mainFile():
00154 self.outputSrv(node)
00155 for n in node.parameters():
00156 self.outputMsg(n.paramType())
00157 n.accept(self)
00158 self.outputMsg(node.returnType())
00159
00160
00161
00162 def getCppTypeText(self, typ, out=False, full=NOT_FULL):
00163 if isinstance(typ, idltype.Base):
00164 return cxx.types.basic_map[typ.kind()]
00165 if isinstance(typ, idltype.String):
00166 return ('char*' if out else 'const char*')
00167 if isinstance(typ, idltype.Declared):
00168 postfix = ('*' if out and cxx.types.variableDecl(typ.decl()) else '')
00169 return self.getCppTypeText(typ.decl(), False, full) + postfix
00170
00171 if isinstance(typ, idlast.Struct):
00172 if full == CPP_FULL:
00173 name = idlutil.ccolonName(typ.scopedName())
00174 elif full == ROS_FULL:
00175 return '_'.join(typ.scopedName())
00176 else:
00177 name = typ.identifier()
00178 return name + ('*' if out and cxx.types.variableDecl(typ) else '')
00179 if isinstance(typ, idlast.Enum) or \
00180 isinstance(typ, idlast.Interface) or \
00181 isinstance(typ, idlast.Operation):
00182 if full == CPP_FULL:
00183 if ( idlutil.ccolonName(typ.scopedName()) == "RTC::LightweightRTObject") :
00184 return idlutil.ccolonName(typ.scopedName())+"_var"
00185 else:
00186 return idlutil.ccolonName(typ.scopedName())
00187 elif full == ROS_FULL:
00188 return '_'.join(typ.scopedName())
00189 else:
00190 return typ.identifier()
00191 if isinstance(typ, idlast.Typedef):
00192 if full == CPP_FULL:
00193 return idlutil.ccolonName(typ.declarators()[0].scopedName())
00194 elif full == ROS_FULL:
00195 return '_'.join(typ.declarators()[0].scopedName())
00196 else:
00197 return typ.declarators()[0].identifier()
00198 if isinstance(typ, idlast.Declarator):
00199 return self.getCppTypeText(typ.alias(), out, full)
00200
00201 return 'undefined'
00202
00203
00204 def getROSTypeText(self, typ):
00205 if isinstance(typ, idltype.Base):
00206 return TypeNameMap[typ.kind()]
00207 if isinstance(typ, idltype.String) or isinstance(typ, idltype.WString):
00208 return 'string'
00209 if isinstance(typ, idltype.Sequence):
00210 etype = typ.seqType()
00211 size = typ.bound()
00212 dim = 1
00213 while not (isinstance(etype, idltype.Base) or isinstance(etype, idltype.String) or isinstance(etype, idltype.WString) or isinstance(etype, idlast.Struct) or isinstance(etype, idlast.Interface) or isinstance(etype, idlast.Enum) or isinstance(etype, idlast.Forward)):
00214 if isinstance(etype, idltype.Declared):
00215 etype = etype.decl()
00216 elif isinstance(etype, idltype.Sequence):
00217 dim += 1
00218 size *= etype.bound()
00219 etype = etype.seqType()
00220 elif isinstance(etype, idlast.Typedef):
00221 arrsize = [size] + etype.declarators()[0].sizes()
00222 if(len(arrsize) != 1): dim += 1
00223 size = reduce(lambda a,b: a*b, arrsize)
00224 etype = etype.aliasType()
00225 elif isinstance(etype, idlast.Declarator):
00226 etype = etype.alias()
00227
00228
00229 else:
00230 return 'undefined'
00231 if( 1 < dim ):
00232 return MultiArrayTypeNameMap[etype.kind()]
00233 return self.getROSTypeText(etype) + ('[]' if size==0 else '[%d]' % size)
00234 if isinstance(typ, idltype.Declared):
00235 return self.getROSTypeText(typ.decl())
00236
00237 if isinstance(typ, idlast.Interface):
00238 return '_'.join(typ.scopedName())
00239 if isinstance(typ, idlast.Struct):
00240 return '_'.join(typ.scopedName())
00241 if isinstance(typ, idlast.Const):
00242 return TypeNameMap[typ.constKind()]
00243 if isinstance(typ, idlast.Enum):
00244 return TypeNameMap[idltype.tk_longlong]
00245 if isinstance(typ, idlast.Union):
00246 return TypeNameMap[idltype.tk_double]
00247 if isinstance(typ, idlast.Typedef):
00248 arraysize = typ.declarators()[0].sizes()
00249 if 0 < len(arraysize):
00250 return self.getROSTypeText(typ.aliasType()) + ('[%d]' % reduce(lambda a,b: a*b, arraysize))
00251 return self.getROSTypeText(typ.aliasType())
00252 if isinstance(typ, idlast.Declarator):
00253 return self.getROSTypeText(typ.alias())
00254 if isinstance(typ, idlast.Forward):
00255 return self.getROSTypeText(typ.fullDecl())
00256
00257 return 'undefined'
00258
00259
00260 def outputMsg(self, typ):
00261 if typ in self.generated_msgs:
00262 return
00263 elif isinstance(typ, idlast.Struct):
00264 for mem in typ.members():
00265 self.outputMsg(mem.memberType())
00266 self.generated_msgs += [typ]
00267 elif isinstance(typ, idlast.Enum) or \
00268 isinstance(typ, idlast.Interface):
00269 self.generated_msgs += [typ]
00270 elif isinstance(typ, idltype.Sequence):
00271 return self.outputMsg(typ.seqType())
00272 elif isinstance(typ, idltype.Declared):
00273 return self.outputMsg(typ.decl())
00274 elif isinstance(typ, idlast.Typedef):
00275 return self.outputMsg(typ.aliasType())
00276 elif isinstance(typ, idlast.Declarator):
00277 return self.outputMsg(typ.alias())
00278 elif isinstance(typ, idlast.Forward):
00279 return self.outputMsg(typ.fullDecl())
00280 else:
00281 return
00282
00283 msgfile = basedir + "/msg/" + self.getCppTypeText(typ,full=ROS_FULL) + ".msg"
00284 if not os.path.exists(basedir):
00285 return
00286 print msgfile
00287 if options.filenames:
00288 return
00289
00290 if os.path.exists(msgfile) and (os.stat(msgfile).st_mtime > os.stat(idlfile).st_mtime) and not options.overwrite:
00291 return
00292
00293 os.system('mkdir -p %s/msg' % basedir)
00294 print >>sys.stderr,"[idl2srv] writing "+msgfile+"...."
00295 fd = open(msgfile, 'w')
00296 if isinstance(typ, idlast.Enum):
00297 for val in typ.enumerators():
00298 fd.write("%s %s=%d\n" % (self.getROSTypeText(typ), val.identifier(), val.value()))
00299 elif isinstance(typ, idlast.Struct):
00300 for mem in typ.members():
00301 fd.write(self.getROSTypeText(mem.memberType()) + " " + mem.declarators()[0].identifier() + "\n")
00302 elif isinstance(typ, idlast.Interface):
00303 for mem in typ.contents():
00304 if isinstance(mem, idlast.Const):
00305 fd.write("%s %s=%s\n" % (self.getROSTypeText(mem), mem.identifier(), mem.value()))
00306 fd.close()
00307
00308
00309 def outputSrv(self, op):
00310
00311 srvfile = basedir + "/srv/" + self.getCppTypeText(op,full=ROS_FULL) + ".srv"
00312 if not os.path.exists(basedir):
00313 return
00314 print srvfile
00315 if options.filenames:
00316 return
00317
00318 if os.path.exists(srvfile) and (os.stat(srvfile).st_mtime > os.stat(idlfile).st_mtime) and not options.overwrite:
00319 return
00320 os.system('mkdir -p %s/srv' % basedir)
00321 args = op.parameters()
00322
00323 print >>sys.stderr, "[idl2srv] writing "+srvfile+"...."
00324 fd = open(srvfile, 'w')
00325 for arg in [arg for arg in args if arg.is_in()]:
00326 fd.write("%s %s\n" % (self.getROSTypeText(arg.paramType()), arg.identifier()))
00327 fd.write("---\n")
00328 if not op.oneway() and op.returnType().kind() != idltype.tk_void:
00329 fd.write("%s operation_return\n" % self.getROSTypeText(op.returnType()))
00330 for arg in [arg for arg in args if arg.is_out()]:
00331 fd.write("%s %s\n" % (self.getROSTypeText(arg.paramType()), arg.identifier()))
00332 fd.close()
00333
00334 def convertFunctionCode(self, interface):
00335 visitor = DependencyVisitor()
00336 interface.accept(visitor)
00337 code = ''
00338
00339 for typ in visitor.multiarray:
00340 msg = MultiArrayTypeNameMap[typ.kind()].replace('/','::')
00341 cpp = cxx.types.basic_map[typ.kind()]
00342 code += multiarray_conversion % (msg,cpp,msg,msg,msg,cpp)
00343
00344 for typ in visitor.allmsg:
00345 if not isinstance(typ, idlast.Struct):
00346 continue
00347
00348 code += 'template<> void convert(%s& in, %s::%s& out){\n' % (self.getCppTypeText(typ, full=CPP_FULL), pkgname, self.getCppTypeText(typ,full=ROS_FULL))
00349 for mem in typ.members():
00350 var = mem.declarators()[0].identifier()
00351 code += ' convert(in.%s, out.%s);\n' % (var, var)
00352 code += '}\n'
00353
00354 code += 'template<> void convert(%s::%s& in, %s& out){\n' % (pkgname, self.getCppTypeText(typ,full=ROS_FULL), self.getCppTypeText(typ, full=CPP_FULL))
00355 for mem in typ.members():
00356 var = mem.declarators()[0].identifier()
00357 code += ' convert(in.%s, out.%s);\n' % (var, var)
00358 code += '}\n'
00359
00360 return code
00361
00362 def ServiceBridgeFunction(self, op, ifname, pkgname):
00363 code = req_code = res_code = ''
00364 params = []
00365 for par in op.parameters():
00366 is_out = par.is_out()
00367 ptype = par.paramType()
00368 var = par.identifier()
00369
00370 if isinstance(ptype.unalias(), idltype.Base) or \
00371 isinstance(ptype.unalias(), idltype.String) or \
00372 isinstance(ptype.unalias(), idltype.Sequence) or \
00373 isinstance(ptype.unalias(), idltype.Declared):
00374 code += ' %s %s;\n' % (self.getCppTypeText(ptype, out=is_out, full=CPP_FULL), var)
00375 params += [var]
00376 if isinstance(ptype.unalias(), idltype.Base) or \
00377 isinstance(ptype.unalias(), idltype.String):
00378 if is_out:
00379 res_code += ' convert(%s, res.%s);\n' % (var, var)
00380 else:
00381 req_code += ' convert(req.%s, %s);\n' % (var, var)
00382 if isinstance(ptype.unalias(), idltype.Sequence):
00383 if is_out:
00384 res_code += ' convert(*%s, res.%s);\n' % (var, var)
00385 else:
00386 req_code += ' convert(req.%s, %s);\n' % (var, var)
00387 if isinstance(ptype.unalias(), idltype.Declared):
00388 if is_out:
00389 ptr = ('*' if cxx.types.variableDecl(ptype.decl()) else '')
00390 res_code += ' convert(%s%s, res.%s);\n' % (ptr, var, var)
00391 if cxx.types.variableDecl(ptype.decl()):
00392 res_code += ' delete %s;\n' % (var)
00393
00394 else:
00395 req_code += ' convert(req.%s, %s);\n' % (var, var)
00396
00397 code += '\n'
00398 code += ' ROS_INFO_STREAM("%s::%s() called");\n' % (ifname, op.identifier())
00399
00400 code += '\n' + req_code + '\n'
00401
00402 code += ' try {\n'
00403
00404 params = ', '.join(params)
00405
00406 if op.oneway() or op.returnType().kind() == idltype.tk_void:
00407 code += ' m_service0->%s(%s);\n' % (op.identifier(), params)
00408 elif isinstance(op.returnType().unalias(), idltype.Base):
00409 code += ' res.operation_return = m_service0->%s(%s);\n' % (op.identifier(), params)
00410 else:
00411 rtype = op.returnType()
00412 if isinstance(rtype.unalias(), idltype.String):
00413 ptr = ''
00414 elif isinstance(rtype.unalias(), idltype.Sequence):
00415 ptr = '*'
00416 elif isinstance(rtype.unalias(), idltype.Declared):
00417 ptr = ('*' if cxx.types.variableDecl(rtype.decl()) else '')
00418 else: ptr = ''
00419 code += ' %s operation_return;\n' % self.getCppTypeText(rtype, out=True, full=CPP_FULL)
00420 code += ' operation_return = m_service0->%s(%s);\n' % (op.identifier(), params)
00421 code += ' convert(%soperation_return, res.operation_return);\n' % ptr
00422
00423 code += ' } catch(CORBA::COMM_FAILURE& ex) {\n'
00424 code += ' ROS_ERROR_STREAM("%s::%s : Caught system exception COMM_FAILURE -- unable to contact the object.");\n' % (ifname, op.identifier())
00425 code += ' return false;\n'
00426 code += ' } catch(CORBA::SystemException&) {\n'
00427 code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::SystemException.");\n' % (ifname, op.identifier())
00428 code += ' return false;\n'
00429 code += ' } catch(CORBA::Exception&) {\n'
00430 code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::Exception.");\n' % (ifname, op.identifier())
00431 code += ' return false;\n'
00432 code += ' } catch(omniORB::fatalException& fe) {\n'
00433 code += ' ROS_ERROR_STREAM("%s::%s : Caught omniORB::fatalException:");\n' % (ifname, op.identifier())
00434 code += ' ROS_ERROR_STREAM(" file: " << fe.file());\n'
00435 code += ' ROS_ERROR_STREAM(" line: " << fe.line());\n'
00436 code += ' ROS_ERROR_STREAM(" mesg: " << fe.errmsg());\n'
00437 code += ' return false;\n'
00438 code += ' }\n'
00439 code += ' catch(...) {\n'
00440 code += ' ROS_ERROR_STREAM("%s::%s : Caught unknown exception.");\n' % (ifname, op.identifier())
00441 code += ' return false;\n'
00442 code += ' }\n'
00443
00444 code += res_code
00445
00446 code += ' ROS_INFO_STREAM("%s::%s() succeeded");\n' % (ifname, op.identifier())
00447
00448 return """bool %s::%s(%s::%s::Request &req, %s::%s::Response &res){\n%s\n return true;\n};\n\n""" % (ifname, op.identifier(), pkgname, self.getCppTypeText(op,full=ROS_FULL), pkgname, self.getCppTypeText(op,full=ROS_FULL), code)
00449
00450
00451 def genBridgeComponent(self, interface):
00452 idlfile = interface.file()
00453 module_name = '%sROSBridge' % interface.identifier()
00454
00455 service_name = interface.identifier()
00456 idl_name = os.path.split(idlfile)[1]
00457 wd = basedir + '/src_gen'
00458
00459 Comp_cpp = wd + '/' + module_name + 'Comp.cpp'
00460 mod_cpp = wd + '/' + module_name + '.cpp'
00461 mod_h = wd + '/' + module_name + '.h'
00462 print Comp_cpp
00463 print mod_cpp
00464 print mod_h
00465 if options.filenames:
00466 return
00467
00468 if all([ os.path.exists(x) and os.stat(x).st_mtime > os.stat(idlfile).st_mtime for x in [Comp_cpp, mod_cpp, mod_h]]) and not options.overwrite:
00469 return
00470
00471
00472 from subprocess import check_output, Popen, PIPE
00473 openrtm_path = Popen(['rospack','find','openrtm_aist'], stdout=PIPE).communicate()[0].rstrip()
00474 if not os.path.exists(os.path.join(openrtm_path, "bin")) :
00475 openrtm_path = os.path.join(check_output(['pkg-config','openrtm-aist','--variable=prefix']).rstrip(),"lib/openrtm_aist")
00476 command = "PATH=%s/bin:$PATH rtc-template -bcxx --module-name=%s --consumer=%s:service0:'%s' --consumer-idl=%s --idl-include=%s" % (openrtm_path, module_name, service_name, service_name, idlfile, idldir)
00477
00478 os.system("mkdir -p %s" % tmpdir)
00479 os.system("mkdir -p %s" % wd)
00480 os.system("cd %s; yes 2> /dev/null | %s > /dev/null" % (tmpdir, command))
00481
00482
00483 operations = [o for o in interface.callables() if isinstance(o, idlast.Operation)]
00484
00485 def addline(src, dest, ref):
00486 idx = dest.find(ref)
00487 idx = dest.find('\n',idx)
00488 return dest[0:idx] + '\n' + src + dest[idx:]
00489
00490 def replaceline(src, dest, ref):
00491 idx1 = dest.find(ref)
00492 idx2 = dest.find('\n',idx1)
00493 return dest[0:idx1] + src + dest[idx2:]
00494
00495
00496
00497
00498 compsrc = open(tmpdir + '/' + module_name + 'Comp.cpp').read()
00499 compsrc = addline(' ros::init(argc, argv, "' + module_name + '", ros::init_options::NoSigintHandler);', compsrc, 'RTC::Manager::init(argc, argv);')
00500 compsrc = replaceline(' comp = manager->createComponent(std::string("'+module_name+'?instance_name="+ros::this_node::getName().substr(1)).c_str()); // skip root name space for OpenRTM instance name', compsrc, ' comp = manager->createComponent("'+module_name+'");')
00501 open(wd + '/' + module_name + 'Comp.cpp', 'w').write(compsrc)
00502
00503
00504
00505
00506 compsrc = open(tmpdir + '/' + module_name + '.cpp').read()
00507
00508 port_name_src = """ nh = ros::NodeHandle("~");
00509 std::string port_name = "service0";
00510 nh.getParam("service_port", port_name);"""
00511 compsrc = addline(port_name_src, compsrc, 'Set service consumers to Ports')
00512 compsrc = compsrc.replace('registerConsumer("service0"','registerConsumer(port_name.c_str()')
00513
00514 compsrc += """
00515 RTC::ReturnCode_t %s::onExecute(RTC::UniqueId ec_id) {
00516 ros::spinOnce();
00517 return RTC::RTC_OK;
00518 }\n\n""" % module_name
00519
00520 compsrc += "RTC::ReturnCode_t %s::onActivated(RTC::UniqueId ec_id) {\n" % module_name
00521 for i in range(len(operations)):
00522 name = operations[i].identifier()
00523 compsrc += ' _srv%d = nh.advertiseService("%s", &%s::%s, this);\n' % (i, name, module_name, name)
00524 compsrc +=" return RTC::RTC_OK;\n}\n\n"""
00525
00526 compsrc += "RTC::ReturnCode_t %s::onDeactivated(RTC::UniqueId ec_id) {\n" % module_name
00527 for i in range(len(operations)):
00528 name = operations[i].identifier()
00529 srvinst = ' _srv%d.shutdown();' % i
00530 compsrc = addline(srvinst, compsrc, 'Unadvertise service')
00531 compsrc +=" return RTC::RTC_OK;\n}\n\n"""
00532
00533 compsrc += convert_functions + self.convertFunctionCode(interface)
00534
00535 for op in operations:
00536 compsrc += self.ServiceBridgeFunction(op, module_name, pkgname)
00537 open(wd + '/' + module_name + '.cpp', 'w').write(compsrc)
00538
00539
00540
00541 compsrc = open(tmpdir + '/' + module_name + '.h').read()
00542 compsrc = re.sub(basedir+"/idl/(.+).h", pkgname+r'/idl/\1.h', compsrc)
00543
00544 compsrc = compsrc.replace('<%s>'%service_name, '<%s>'%idlutil.ccolonName(interface.scopedName()))
00545
00546 incs = ['', '// ROS', '#include <ros/ros.h>']
00547 incs += ['#include <%s/%s.h>' % (pkgname, self.getCppTypeText(op,full=ROS_FULL)) for op in operations]
00548 incs = '\n'.join(incs)
00549 compsrc = addline(incs, compsrc, '#define')
00550
00551 compsrc = '\n'.join([ (a.replace('//','') if ('RTC::ReturnCode_t onExecute' in a) else a) for a in compsrc.split('\n')])
00552 compsrc = '\n'.join([ (a.replace('//','') if ('RTC::ReturnCode_t onActivated' in a) else a) for a in compsrc.split('\n')])
00553 compsrc = '\n'.join([ (a.replace('//','') if ('RTC::ReturnCode_t onDeactivated' in a) else a) for a in compsrc.split('\n')])
00554
00555 srvfunc = [' bool %s(%s::%s::Request &req, %s::%s::Response &res);' % (op.identifier(), pkgname, self.getCppTypeText(op,full=ROS_FULL), pkgname, self.getCppTypeText(op,full=ROS_FULL)) for op in operations]
00556 srvfunc = '\n'.join(srvfunc)
00557 compsrc = addline(srvfunc, compsrc, 'public:')
00558
00559 defsrv = " ros::NodeHandle nh;\n";
00560 defsrv += " ros::ServiceServer " + ', '.join(['_srv%d' % i for i in range(len(operations))]) + ';'
00561 compsrc = addline(defsrv, compsrc, 'private:')
00562
00563 open(wd + '/' + module_name + '.h', 'w').write(compsrc)
00564
00565
00566
00567 return
00568
00569
00570 class DependencyVisitor (idlvisitor.AstVisitor):
00571 def __init__(self):
00572 self.allmsg = []
00573 self.multiarray = []
00574 self.checked = []
00575
00576 def visitInterface(self, node):
00577 if node.mainFile():
00578 for n in node.callables():
00579 n.accept(self)
00580
00581 def checkBasicType(self, node):
00582 classes = [idltype.Base, idltype.String, idltype.WString]
00583 classes += [idlast.Const, idlast.Enum, idlast.Union]
00584 if not (any([isinstance(node, cls) for cls in classes]) or node in self.checked):
00585 self.checked += [node]
00586 node.accept(self)
00587
00588 def visitOperation(self, node):
00589 types = [p.paramType() for p in node.parameters()]
00590 types += [node.returnType()]
00591 for n in types:
00592 self.checkBasicType(n)
00593
00594 def visitStruct(self, node):
00595 for mem in node.members():
00596 self.checkBasicType(mem.memberType())
00597 if not node in self.allmsg:
00598 self.allmsg += [node]
00599
00600 def visitSequenceType(self, node):
00601 etype = node.seqType()
00602 dim = 1
00603 while not (isinstance(etype, idltype.Base) or isinstance(etype, idltype.String) or isinstance(etype, idltype.WString) or isinstance(etype, idlast.Struct) or isinstance(etype, idlast.Interface) or isinstance(etype, idlast.Enum) or isinstance(etype, idlast.Forward)):
00604 if isinstance(etype, idltype.Sequence):
00605 dim += 1
00606 etype = etype.seqType()
00607 elif isinstance(etype, idltype.Declared):
00608 etype = etype.decl()
00609 elif isinstance(etype, idlast.Typedef):
00610 if len(etype.declarators()[0].sizes()) != 0 : dim += 1
00611 etype = etype.aliasType()
00612 elif isinstance(etype, idlast.Declarator):
00613 etype = etype.alias()
00614
00615
00616 if 1 < dim and isinstance(etype, idltype.Base) and (not etype in self.multiarray) :
00617 self.multiarray += [etype]
00618 self.checkBasicType(etype)
00619 def visitDeclaredType(self, node):
00620 self.checkBasicType(node.decl())
00621 def visitTypedef(self, node):
00622 self.checkBasicType(node.aliasType())
00623 def visitDeclarator(self, node):
00624 self.checkBasicType(node.alias())
00625 def visitForward(self, node):
00626 self.checkBasicType(node.fullDecl())
00627
00628
00629
00630 class InterfaceNameVisitor (idlvisitor.AstVisitor):
00631 def visitAST(self, node):
00632 for n in node.declarations():
00633 n.accept(self)
00634 def visitModule(self, node):
00635 for n in node.definitions():
00636 n.accept(self)
00637 def visitInterface(self, node):
00638 if node.mainFile():
00639 print node.identifier()
00640 if options.interfaces:
00641 pass
00642
00643
00644 if __name__ == '__main__':
00645 global options, basedir, pkgname, tmpdir
00646
00647 parser = OptionParser()
00648 parser.add_option("-i", "--idl", dest="idlfile",
00649 help="target idl file", metavar="FILE")
00650 parser.add_option("-I", "--include-dirs", dest="idlpath", metavar="PATHLIST",
00651 help="list of directories to check idl include")
00652 parser.add_option("-o", "--overwrite", action="store_true",
00653 dest="overwrite", default=False,
00654 help="overwrite all generate files")
00655 parser.add_option("--filenames", action="store_true",
00656 dest="filenames", default=False,
00657 help="print filenames to generate")
00658 parser.add_option("--interfaces", action="store_true",
00659 dest="interfaces", default=False,
00660 help="print interface names")
00661 parser.add_option("--package-name", action="store", type="string",
00662 dest="package_name", default=False,
00663 help="overwrite package name")
00664 parser.add_option("--tmpdir", action="store", type="string",
00665 dest="tmpdir", default="/tmp/idl2srv",
00666 help="tmporary directory")
00667 (options, args) = parser.parse_args()
00668
00669 idlfile = options.idlfile
00670 if not os.path.exists(idlfile):
00671 exit
00672 tmpdir = options.tmpdir
00673 idlfile = os.path.abspath(idlfile)
00674 idldir = os.path.split(idlfile)[0]
00675 basedir = os.path.split(idldir)[0]
00676 if options.package_name:
00677 pkgname = options.package_name
00678 else:
00679 pkgname = os.path.split(basedir)[1]
00680
00681
00682 if options.idlpath:
00683 pathlist = options.idlpath.strip('"')
00684 option = ' '.join(['-I'+d for d in filter(None, pathlist.split(' '))])
00685 else:
00686 option = ''
00687
00688 fd = os.popen('/usr/bin/omnicpp %s "%s"' % (option, idlfile), 'r')
00689 try:
00690 tree = _omniidl.compile(fd, idlfile)
00691 except:
00692 tree = _omniidl.compile(fd)
00693
00694
00695 if options.interfaces:
00696 tree.accept(InterfaceNameVisitor())
00697 else:
00698 tree.accept(ServiceVisitor())
00699 tree.accept(InterfaceNameVisitor())
00700