00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 import string
00021 from xml.dom.minidom import Document
00022 from xml.dom import minidom
00023 import sys
00024 from numpy import array,pi
00025 import re, copy
00026
00027 ZERO_THRESHOLD=0.000000001
00028
00029 def reindent(s, numSpaces):
00030 """Reindent a string for tree structure pretty printing."""
00031 s = string.split(s, '\n')
00032 s = [(numSpaces * ' ') + line for line in s]
00033 s = string.join(s, '\n')
00034 return s
00035
00036 def add(doc, base, element):
00037 """Add an XML element for URDF export"""
00038 if element is not None:
00039 base.appendChild( element.to_xml(doc) )
00040
00041 def add_openrave(doc, base, element):
00042 """Add an XML element for OpenRAVE XML export"""
00043 if element is not None:
00044
00045 newelements=element.to_openrave_xml(doc)
00046 if hasattr(newelements, '__iter__'):
00047 for e in newelements:
00048 base.appendChild(e)
00049 else:
00050 base.appendChild(newelements)
00051
00052 def pfloat(x):
00053 """Print float value as string"""
00054 return "{0}".format(x).rstrip('.')
00055
00056 def to_string(data=None):
00057 """Convert data fromvarious types to urdf string format"""
00058 if data is None:
00059 return None
00060 if hasattr(data, '__iter__'):
00061 outlist=[]
00062 for a in data:
00063 try:
00064 if abs(a)>ZERO_THRESHOLD:
00065 outlist.append(pfloat(a))
00066 else:
00067 outlist.append("0")
00068 except TypeError:
00069 outlist.append(pfloat(a))
00070 return ' '.join(outlist)
00071
00072 elif type(data) == type(0.0):
00073 return pfloat(data if abs(data)>ZERO_THRESHOLD else 0)
00074 elif type(data) != type(''):
00075 return str(data)
00076 return data
00077
00078 def set_attribute(node, name, value):
00079 """Set an attribute on an XML node, converting data to string format"""
00080 node.setAttribute(name, to_string(value))
00081
00082 def set_content(doc,node,data):
00083 """Create a text node and add it to the current element"""
00084 if data is None:
00085 return
00086 node.appendChild(doc.createTextNode(to_string(data)))
00087
00088 def short(doc, name, key, value):
00089 element = doc.createElement(name)
00090 set_attribute(element, key, value)
00091 return element
00092
00093 def create_element(doc, name, contents=None, key=None, value=None):
00094 element = doc.createElement(name)
00095 if contents is not None:
00096 set_content(doc,element,contents)
00097 if key is not None:
00098 set_attribute(element, key, value)
00099
00100 return element
00101
00102 def create_child(doc,name,contents=None,key=None,value=None):
00103 doc.appendChild(create_element(doc, name, contents=None, key=None, value=None))
00104
00105 def children(node):
00106 children = []
00107 for child in node.childNodes:
00108 if child.nodeType is node.TEXT_NODE \
00109 or child.nodeType is node.COMMENT_NODE:
00110 continue
00111 else:
00112 children.append(child)
00113 return children
00114
00115
00116
00117
00118
00119
00120
00121 class Gazebo(object):
00122 """
00123 Represents any type of gazebo tag :
00124 - global gazebo tag inside a robot tag
00125 - gazebo tags referencing links
00126 - gazebo tags instantiating plugins and sensors
00127 """
00128 ATTRIBUTE_NAMES=['material','turnGravityOff','dampingFactor','maxVel',\
00129 'minDepth','mu1','mu2','fdir1','kp','kd','selfCollide','maxContacts','laserRetro']
00130 def __init__(self,reference=None,material=None,gravity=None,dampingFactor=None,maxVel=None,minDepth=None,mu1=None,mu2=None,fdir1=None,kp=None,kd=None,selfCollide=None,maxContacts=None,laserRetro=None,plugin=[],sensor=[]):
00131 self.reference = reference
00132 self.material = material
00133 self.gravity = gravity
00134 self.dampingFactor = dampingFactor
00135 self.maxVel = maxVel
00136 self.minDepth = minDepth
00137 self.mu1 = mu1
00138 self.mu2 = mu2
00139 self.fdir1 = fdir1
00140 self.kp = kp
00141 self.kd = kd
00142 self.selfCollide = selfCollide
00143 self.maxContacts = maxContacts
00144 self.laserRetro = laserRetro
00145 self.plugins = plugin
00146 self.sensors = sensor
00147 @staticmethod
00148 def parse(node, verbose=True):
00149 gaze = Gazebo()
00150 gaze.sensors=[]
00151 gaze.plugins=[]
00152 if node.hasAttribute('reference'):
00153 gaze.reference = node.getAttribute('reference')
00154 for child in children(node):
00155 for attribute in gaze.ATTRIBUTE_NAMES:
00156 if child.localName == attribute :
00157 if attribute == 'sensor':
00158 gaze.sensors.append(Sensor.parse(child,verbose))
00159 else:
00160 setattr(gaze,attribute,str(child.childNodes[0].nodeValue))
00161
00162 else:
00163 for child in children(node):
00164 if child.localName == 'plugin':
00165 gaze.plugins.append(BasePlugin.parse(child,verbose))
00166 return gaze
00167
00168 def to_xml(self, doc):
00169 xml = doc.createElement('gazebo')
00170 if self.reference !=None:
00171 set_attribute(xml, 'reference', self.reference)
00172 for attribute in self.ATTRIBUTE_NAMES:
00173 value = getattr(self,attribute)
00174 if value != None:
00175 xml.appendChild(createElement(doc,attribute,value))
00176
00177 if len(self.sensors) !=0:
00178 for sensor in self.sensors:
00179 add(doc,xml, sensor)
00180 else:
00181 if len(self.plugins)!=0:
00182 for plugin in self.plugins:
00183 add(doc,xml, plugin)
00184
00185 return xml
00186
00187 def __str__(self):
00188 s = ""
00189 if self.reference !=None:
00190 s += "Reference: {0}\n".format(self.reference)
00191 for name in self.ATTRIBUTE_NAMES:
00192 value = getattr(self,name)
00193 if value != None:
00194 s+= name.title() + ":\n"
00195 s+= reindent(str(value,1)) + '\n'
00196 for sensor in self.sensors:
00197 s+='Sensor'
00198 s+=reindent(str(sensor), 1) + "\n"
00199 else:
00200 for plugin in self.plugins:
00201 s+='Plugin'
00202 s+=reindent(str(plugin), 1) + "\n"
00203 return s
00204
00205
00206
00207
00208
00209 class BasePlugin(object):
00210 """
00211 Abstract class used to parse plugin tags and instantiating
00212 inherited plugin accordingly
00213
00214 """
00215 ATTRIBUTE_NAMES = []
00216 def __init__(self,name=None,filename=None,attributes=None):
00217 self.name = name
00218 self.filename = filename
00219 self.attributes = attributes
00220
00221 @classmethod
00222 def parse(cls,node, verbose=True):
00223 for child in children(node):
00224 if child.localName == 'cameraName':
00225 plug = CameraPlugin(node.getAttribute('name'),node.getAttribute('filename'))
00226 break
00227 elif child.localName == 'bumperTopicName':
00228 plug = BumperPlugin(node.getAttribute('name'),node.getAttribute('filename'))
00229 break
00230 elif child.localName == 'leftFrontJoint':
00231 plug = DrivePlugin(node.getAttribute('name'),node.getAttribute('filename'))
00232 break
00233 elif child.localName == 'odometryTopic':
00234 plug = OdometryPlugin(node.getAttribute('name'),node.getAttribute('filename'))
00235 break
00236 elif child.localName == 'height':
00237 plug = VideoPlugin(node.getAttribute('name'),node.getAttribute('filename'))
00238 break
00239 elif child.localName == 'mimicJoint':
00240 plug = MimicJointPlugin(node.getAttribute('name'),node.getAttribute('filename'))
00241 break
00242 elif child.localName == 'robotSimType':
00243 plug = SimuPlugin(node.getAttribute('name'),node.getAttribute('filename'))
00244 else:
00245 plug = ImuLaserPlugin()
00246 for child in children(node):
00247 if child.localName in cls.ATTRIBUTE_NAMES:
00248 setattr(plug,child.localName,str(child.childNodes[0].nodeValue))
00249 return plug
00250
00251 def to_xml(self,doc):
00252 xml = doc.createElement('plugin')
00253 if self.name != None:
00254 set_attribute(xml, 'name', self.name)
00255 if self.filename != None:
00256 set_attribute(xml,'filename',self.filename)
00257 for attribute in self.ATTRIBUTE_NAMES:
00258 value = getattr(self,attribute)
00259 if value != None:
00260 xml.appendChild(create_element(doc,attribute,value))
00261 return xml
00262
00263 def __str__(self):
00264 if self.name != None:
00265 s = 'Name: {0}\n'.format(self.name)
00266 if self.filename != None:
00267 s += 'Filename: {0}\n'.format(self.filename)
00268 for attribute in self.ATTRIBUTE_NAMES:
00269 value = getattr(self,attribute)
00270 if value != None:
00271 s+= name.title() + ":\n"
00272 s+=reindent(str(value),1) + '\n'
00273
00274 class SimuPlugin(BasePlugin):
00275 """
00276 Define the simulation plugin used to initialize simulation parameters in gazebo
00277 plugin
00278 robotNamespace
00279 robotSimType
00280 """
00281 ATTRIBUTE_NAMES = [ 'robotNamespace', 'robotSimType']
00282 def __init__(self,name=None,filename=None,robot_namespace=None,robot_sim_type=None):
00283 super(SimuPlugin, self).__init__(name,filename,{'robotNamespace': robot_namespace,
00284 'robotSimType': robot_sim_type})
00285
00286
00287 class MimicJointPlugin(BasePlugin):
00288 """
00289 Defines the plugin used to define mimic joints (joint not commanded directly by an actuator but depending to another joint position)
00290 plugin
00291 joint
00292 mimicjoint
00293 multiplier
00294 offset
00295 """
00296 ATTRIBUTE_NAMES = ['joint', 'mimicJoint','multiplier','offset']
00297 def __init__(self,name=None,filename=None,joint=None,mimic_joint=None,multiplier=None,offset=None):
00298 super(MimicJointPlugin, self).__init__(name,filename,{'joint': joint,
00299 'mimicJoint': mimic_joint,
00300 'multiplier': multiplier,
00301 'offset': offset })
00302
00303 class BumperPlugin(BasePlugin):
00304 """
00305 Class defining the plugin used to simulate bumpers on a robot
00306 plugin
00307 bumperTopicName
00308 frameName
00309 alwaysOn
00310 updateRate
00311 """
00312 ATTRIBUTE_NAMES = ['bumperTopicName', 'frameName','alwaysOn','updateRate']
00313 def __init__(self,name=None,filename=None,bumper_topic_name=None,frame_name=None,always_on=None,update_rate=None):
00314 super(BumperPlugin, self).__init__(name,filename,{'bumperTopicName': bumper_topic_name,
00315 'frameName': frame_name,
00316 'alwaysOn': always_on,
00317 'updateRate': update_rate })
00318
00319 class ImuLaserPlugin(BasePlugin):
00320 """
00321 Class defining the plugin used to IMU on a robot
00322 note: gazebo official IMU plugin has some weird behaviour, an homemade plugin is being developped
00323
00324 plugin
00325 robotNamespace
00326 topicName
00327 gaussianNoise
00328 xyzOffset
00329 rpyOffset
00330 alwaysOn
00331 updateRate
00332 frameName
00333 hokuyoMinIntensity
00334 frameId
00335 accelGaussianNoise
00336 headingGaussianNoise
00337 rateGaussianNoise
00338 bodyName
00339 """
00340 ATTRIBUTE_NAMES = ['robotNamespace', 'topicName','gaussianNoise','xyzOffset','rpyOffset','alwaysOn','updateRate','frameName', 'hokuyoMinIntensity','frameId','accelGaussianNoise','headingGaussianNoise','rateGaussianNoise','bodyName']
00341 def __init__(self,name=None,filename=None,robot_namespace=None,topic_name=None,gaussian_noise=None,xyz_offset=None,rpy_offset=None, always_on=None,update_rate=None, hokuyo_min_intensity=None, frame_id=None,accel_gaussian_noise=None,heading_gaussian_noise=None,rate_gaussian_noise=None,body_name=None):
00342 super(ImuLaserPlugin, self).__init__(name,filename,{'robotNamespace': robot_namespace,
00343 'topicName': topic_name,
00344 'gaussianNoise': gaussian_noise,
00345 'xyzOffset': xyz_offset,
00346 'rpyOffset': rpy_offset,
00347 'alwaysOn':always_on,
00348 'updateRate':update_rate,
00349 'hokuyoMinIntensity':hokuyo_min_intensity,
00350 'frameId':frame_id,
00351 'accelGaussianNoise':accel_gaussian_noise,
00352 'headingGaussianNoise':heading_gaussian_noise,
00353 'rateGaussianNoise':rate_gaussian_noise,
00354 'bodyName':body_name})
00355
00356 class VideoPlugin(BasePlugin):
00357 """
00358 Class defining the plugin used to simulate video on a robot
00359 plugin
00360 topicName
00361 height
00362 width
00363 alwaysOn
00364 updateRate
00365 """
00366 ATTRIBUTE_NAMES = ['topicName','height','width','alwaysOn','updateRate']
00367 def __init__(self,name=None,filename=None,topic_name=None,height=None,width=None, always_on=None,update_rate=None):
00368 super(VideoPlugin, self).__init__(name,filename,{'topicName': topic_name,
00369 'height': height,
00370 'width': width,
00371 'alwaysOn':always_on,
00372 'updateRate':update_rate})
00373
00374 class OdometryPlugin(BasePlugin):
00375 """
00376 Class defining the plugin used to odometry on a robot
00377 plugin
00378 commandTopic
00379 odometryTopic
00380 odometryFrame
00381 odometryRate
00382 robotBaseFrame
00383 alwaysOn
00384 update_rate
00385 """
00386 ATTRIBUTE_NAMES = ['commandTopic','odometryTopic','odometryFrame','odometryRate','robotBaseFrame','alwaysOn','updateRate']
00387 def __init__(self,name=None,filename=None,command_topic=None,odometry_topic=None,odometry_frame=None,odometry_rate=None,robot_base_frame=None, always_on=None,update_rate=None):
00388 super(OdometryPlugin, self).__init__(name,filename,{'commandTopic': command_topic,
00389 'odometryTopic': odometry_topic,
00390 'odometryFrame': odometry_frame,
00391 'odometryRate': odometry_rate,
00392 'robotBaseFrame': robot_base_frame,
00393 'alwaysOn':always_on,
00394 'updateRate':update_rate})
00395
00396
00397
00398
00399
00400
00401 class Sensor(object):
00402 """
00403 Class defining the sensors and the related plugins
00404 sensor
00405 type
00406 updateRate
00407 camera
00408 ...
00409 plugin
00410 ...
00411 pose
00412 visualize
00413 ray
00414 """
00415 def __init__(self,name=None,type=None,update_rate=None,camera=[],plugin=[],pose=None,visualize=None,ray=None):
00416 self.name = name
00417 self.type = type
00418 self.update_rate = update_rate
00419 self.cameras = camera
00420 self.plugins = plugin
00421 self.pose = pose
00422 self.visualize = visualize
00423 self.ray = ray
00424
00425 @staticmethod
00426 def parse(node, verbose=True):
00427 sens = Sensor(node.getAttribute('name'),node.getAttribute('type'))
00428 sens.plugins = []
00429 sens.cameras = []
00430 for child in children(node):
00431 if child.localName == 'ray':
00432 sens.ray = Ray.parse(child,verbose)
00433 if child.localName == 'update_rate':
00434 sens.update_rate = str(child.childNodes[0].nodeValue)
00435 if child.localName == 'camera':
00436 sens.cameras.append(CameraSensor.parse(child,verbose))
00437 if child.localName == 'plugin':
00438 sens.plugins.append(BasePlugin.parse(child,verbose))
00439 if child.localName == 'pose':
00440 sens.pose = array([float(x) for x in child.childNodes[0].nodeValue.split(' ')])
00441 if child.localName == 'visualize':
00442 sens.visualize = str(child.childNodes[0].nodeValue)
00443 return sens
00444
00445 def to_xml(self, doc):
00446 xml = doc.createElement('sensor')
00447 set_attribute(xml, 'name', self.name)
00448 set_attribute(xml, 'type', self.type)
00449 if self.update_rate != None:
00450 xml.appendChild(create_element(doc, 'update_rate', self.update_rate))
00451 for camera in self.cameras:
00452 add( doc, xml, camera)
00453 for plugin in self.plugins:
00454 add(doc,xml,plugin)
00455 if self.pose != None:
00456 xml.appendChild(create_element(doc, 'pose', self.pose))
00457
00458 if self.visualize != None:
00459 xml.appendChild(create_element(doc, 'visualize', self.visualize))
00460 if self.ray != None:
00461 add(doc,xml,self.ray)
00462 return xml
00463
00464 def __str__(self):
00465 s = ""
00466 s += "Name: {0}\n".format(self.name)
00467 s += "Type: {0}\n".format(self.type)
00468 if self.update_rate != None:
00469 s += "Update_rate:\n"
00470 s += reindent(str(self.update_rate),1) + "\n"
00471 for camera in self.cameras:
00472 s+='Camera'
00473 s+=reindent(str(camera), 1) + "\n"
00474 for plugin in self.plugins:
00475 s+='Plugin'
00476 s+=reindent(str(plugin), 1) + "\n"
00477
00478 if self.visualize != None:
00479 s += "Visualize:\n"
00480 s += reindent(str(self.visualize),1) + "\n"
00481
00482 if self.ray != None:
00483 s += "Ray:\n"
00484 s += reindent(str(self.ray),1) + "\n"
00485 return s
00486
00487 class CameraSensor(object):
00488 """
00489 Class defining the camera sensor. This can instantiate specific plugin according to the type of camera (RGB,Stereo,Depth...)
00490 camera
00491 horizontal_fov
00492 image
00493 clip
00494 noise
00495 """
00496 def __init__(self,name=None,horizontal_fov=None,image=None,clip=None,noise=None):
00497 self.name = name
00498 self.horizontal_fov = horizontal_fov
00499 self.image = image
00500 self.clip = clip
00501 self.noise = noise
00502
00503 @staticmethod
00504 def parse(node, verbose=True):
00505 sens = CameraSensor()
00506 if node.hasAttribute('name'):
00507 sens.name = node.getAttribute('name')
00508 for child in children(node):
00509 if child.localName == 'horizontal_fov':
00510 sens.horizontal_fov = str(child.childNodes[0].nodeValue)
00511 elif child.localName == 'image':
00512 sens.image = Image.parse(child,verbose)
00513 elif child.localName == 'clip':
00514 sens.clip = Clip.parse(child,verbose)
00515 elif child.localName == 'noise':
00516 sens.noise = Noise.parse(child,verbose)
00517 return sens
00518
00519 def to_xml(self, doc):
00520 xml = doc.createElement('camera')
00521 if self.name!= None:
00522 set_attribute(xml, 'name', self.name)
00523 if self.horizontal_fov != None :
00524 xml.appendChild(create_element(doc, 'horizontal_fov', self.horizontal_fov))
00525 if self.image != None:
00526 add( doc, xml, self.image)
00527 if self.clip != None :
00528 add( doc, xml, self.clip)
00529 if self.noise != None :
00530 add( doc, xml, self.noise)
00531 return xml
00532
00533 def __str__(self):
00534 s = ''
00535 if self.name!= None:
00536 s = "Name: {0}\n".format(self.name)
00537 if self.horizontal_fov != None:
00538 s += "Horizontal_fov:\n"
00539 s += reindent(str(self.horizontal_fov),1) + '\n'
00540 if self.image != None:
00541 s += "Image:\n"
00542 s += reindent(str(self.image),1) + '\n'
00543 if self.clip != None :
00544 s += "Clip:\n"
00545 s += reindent(str(self.clip),1) + '\n'
00546 if self.noise != None:
00547 s += "Noise:\n"
00548 s += reindent(str(self.noise),1) + '\n'
00549 return s
00550
00551
00552
00553
00554
00555
00556
00557 class Clip(object):
00558 """
00559 Class defining the clip element used in sensors (for range or beaming sensors)
00560 structure:
00561 clip
00562 near
00563 far
00564 """
00565 def __init__(self,near=None,far=None):
00566 self.near = near
00567 self.far = far
00568
00569 @staticmethod
00570 def parse(node, verbose=True):
00571 clip = Clip()
00572 for child in children(node):
00573 if child.localName == 'near':
00574 clip.near = str(child.childNodes[0].nodeValue)
00575 elif child.localName == 'far':
00576 clip.far = str(child.childNodes[0].nodeValue)
00577 return clip
00578
00579 def to_xml(self, doc):
00580 xml = doc.createElement('clip')
00581 if self.near != None :
00582 xml.appendChild(create_element(doc, 'near', self.near))
00583 if self.far != None:
00584 xml.appendChild(create_element(doc, 'far', self.far))
00585 return xml
00586
00587 def __str__(self):
00588 s = ''
00589 if self.near != None:
00590 s += "Near:\n"
00591 s += reindent(str(self.near),1) + '\n'
00592 if self.far != None:
00593 s += "Far:\n"
00594 s += reindent(str(self.far),1) + '\n'
00595 return s
00596
00597 class Image(object):
00598 """
00599 Class defining the image element used in camera sensor
00600 image
00601 height
00602 width
00603 format
00604 """
00605 def __init__(self,height=None,width=None,format=None):
00606 self.height = height
00607 self.width = width
00608 self.format = format
00609
00610 @staticmethod
00611 def parse(node, verbose=True):
00612 img = Image()
00613 for child in children(node):
00614 if child.localName == 'height':
00615 img.height = str(child.childNodes[0].nodeValue)
00616 elif child.localName == 'width':
00617 img.width = str(child.childNodes[0].nodeValue)
00618 elif child.localName == 'format':
00619 img.format = str(child.childNodes[0].nodeValue)
00620 return img
00621
00622 def to_xml(self, doc):
00623 xml = doc.createElement('image')
00624 if self.height != None :
00625 xml.appendChild(create_element(doc, 'height', self.height))
00626 if self.width != None :
00627 xml.appendChild(create_element(doc, 'width', self.width))
00628 if self.format != None:
00629 xml.appendChild(create_element(doc, 'format', self.format))
00630 return xml
00631
00632 def __str__(self):
00633 s = ''
00634 if self.height != None:
00635 s += "Height:\n"
00636 s += reindent(str(self.height),1) + '\n'
00637 if self.width != None:
00638 s += "Width:\n"
00639 s += reindent(str(self.width),1) + '\n'
00640 if self.format != None:
00641 s += "Format:\n"
00642 s += reindent(str(self.format),1) + '\n'
00643 return s
00644
00645
00646 class Noise(object):
00647 """
00648 Class defining the noise element used in most sensors definition
00649 noise
00650 type
00651 mean
00652 stddev
00653 """
00654 def __init__(self,type=None,mean=None,stddev=None):
00655 self.type = type
00656 self.mean = mean
00657 self.stddev = stddev
00658
00659 @staticmethod
00660 def parse(node, verbose=True):
00661 noise = Noise()
00662 for child in children(node):
00663 if child.localName == 'type':
00664 noise.type = str(child.childNodes[0].nodeValue)
00665 elif child.localName == 'mean':
00666 noise.mean = str(child.childNodes[0].nodeValue)
00667 elif child.localName == 'stddev':
00668 noise.stddev = str(child.childNodes[0].nodeValue)
00669 return noise
00670
00671 def to_xml(self, doc):
00672 xml = doc.createElement('noise')
00673 if self.type != None :
00674 xml.appendChild(create_element(doc, 'type', self.type))
00675 if self.mean != None :
00676 xml.appendChild(create_element(doc, 'mean', self.mean))
00677 if self.stddev != None:
00678 xml.appendChild(create_element(doc, 'stddev', self.stddev))
00679 return xml
00680
00681 def __str__(self):
00682 s = ''
00683 if self.type != None:
00684 s += "Type:\n"
00685 s += reindent(str(self.type),1) + '\n'
00686 if self.mean != None:
00687 s += "Mean:\n"
00688 s += reindent(str(self.mean),1) + '\n'
00689 if self.stddev != None:
00690 s += "Stddev:\n"
00691 s += reindent(str(self.stddev),1) + '\n'
00692 return s
00693
00694
00695 class Ray(object):
00696 """
00697 Class defining the ray element used in most range sensors
00698 ray
00699 scan
00700 range
00701 noise
00702 """
00703 def __init__(self,scan=None,range=None,noise=None):
00704 self.scan = scan
00705 self.range = range
00706 self.noise = noise
00707
00708 @staticmethod
00709 def parse(node, verbose=True):
00710 ray = Ray()
00711 for child in children(node):
00712 if child.localName == 'scan':
00713 ray.scan = Scan.parse(child,verbose)
00714 elif child.localName == 'range':
00715 ray.range = Range.parse(child,verbose)
00716 elif child.localName == 'noise':
00717 ray.noise = Noise.parse(child,verbose)
00718 return ray
00719
00720 def to_xml(self, doc):
00721 xml = doc.createElement('ray')
00722 if self.scan != None :
00723 add( doc, xml, self.scan)
00724 if self.range != None :
00725 add( doc, xml, self.range)
00726 if self.noise != None:
00727 add( doc, xml, self.noise)
00728 return xml
00729
00730 def __str__(self):
00731 s = ''
00732 if self.scan != None:
00733 s += "Scan:\n"
00734 s += reindent(str(self.scan),1) + '\n'
00735 if self.range != None:
00736 s += "Range:\n"
00737 s += reindent(str(self.range),1) + '\n'
00738 if self.noise != None:
00739 s += "Noise:\n"
00740 s += reindent(str(self.noise),1) + '\n'
00741 return s
00742
00743 class Range(object):
00744 """
00745 Class defining the range element used in most range sensors (mostly LASERs)
00746 range:
00747 min
00748 max
00749 resolution
00750 """
00751 def __init__(self,min=None,max=None,resolution=None):
00752 self.min = min
00753 self.max = max
00754 self.resolution = resolution
00755
00756 @staticmethod
00757 def parse(node, verbose=True):
00758 rang = Range()
00759 for child in children(node):
00760 if child.localName == 'min':
00761 rang.min = str(child.childNodes[0].nodeValue)
00762 elif child.localName == 'max':
00763 rang.max = str(child.childNodes[0].nodeValue)
00764 elif child.localName == 'resolution':
00765 rang.resolution = str(child.childNodes[0].nodeValue)
00766 return rang
00767
00768 def to_xml(self, doc):
00769 xml = doc.createElement('range')
00770 if self.min != None :
00771 xml.appendChild(create_element(doc, 'min', self.min))
00772 if self.max != None :
00773 xml.appendChild(create_element(doc, 'max', self.max))
00774 if self.resolution != None:
00775 xml.appendChild(create_element(doc, 'resolution', self.resolution))
00776 return xml
00777
00778 def __str__(self):
00779 s = ''
00780 if self.min != None:
00781 s += "Min:\n"
00782 s += reindent(str(self.min),1) + '\n'
00783 if self.max != None:
00784 s += "Max:\n"
00785 s += reindent(str(self.max),1) + '\n'
00786 if self.resolution != None:
00787 s += "Resolution:\n"
00788 s += reindent(str(self.resolution),1) + '\n'
00789 return s
00790
00791 class Scan(object):
00792 """
00793 Class defining the scan element used in most range sensors (mostly LASERs)
00794 scan
00795 horizontal
00796 """
00797 def __init__(self,horizontal=None):
00798 self.horizontal = horizontal
00799
00800 @staticmethod
00801 def parse(node, verbose=True):
00802 scan = Scan()
00803 for child in children(node):
00804 if child.localName == 'horizontal':
00805 scan.horizontal = Horizontal.parse(child,verbose)
00806 return scan
00807
00808 def to_xml(self, doc):
00809 xml = doc.createElement('scan')
00810 if self.horizontal != None :
00811 add( doc, xml, self.horizontal)
00812 return xml
00813
00814 def __str__(self):
00815 s = ''
00816 if self.horizontal != None:
00817 s += "Horizontal:\n"
00818 s += reindent(str(self.horizontal),1) + '\n'
00819 return s
00820
00821
00822 class Horizontal(object):
00823 """
00824 Class defining the horizontal element used in most range sensors(mostly LASERs)
00825 horizontal
00826 samples
00827 resolution
00828 min_angle
00829 max_angle
00830 """
00831 def __init__(self,samples=None,resolution=None,min_angle=None,max_angle=None):
00832 self.samples = samples
00833 self.resolution = resolution
00834 self.min_angle = min_angle
00835 self.max_angle = max_angle
00836
00837 @staticmethod
00838 def parse(node, verbose=True):
00839 hori = Horizontal()
00840 for child in children(node):
00841 if child.localName == 'samples':
00842 hori.samples = str(child.childNodes[0].nodeValue)
00843 elif child.localName == 'resolution':
00844 hori.resolution = str(child.childNodes[0].nodeValue)
00845 elif child.localName == 'min_angle':
00846 hori.min_angle = str(child.childNodes[0].nodeValue)
00847
00848 elif child.localName == 'max_angle':
00849 hori.max_angle = str(child.childNodes[0].nodeValue)
00850 return hori
00851
00852 def to_xml(self, doc):
00853 xml = doc.createElement('horizontal')
00854 if self.samples != None :
00855 xml.appendChild(create_element(doc, 'samples', self.samples))
00856 if self.resolution != None :
00857 xml.appendChild(create_element(doc, 'resolution', self.resolution))
00858 if self.min_angle != None :
00859 xml.appendChild(create_element(doc, 'min_angle', self.min_angle))
00860 if self.max_angle != None :
00861 xml.appendChild(create_element(doc, 'max_angle', self.samples))
00862
00863 return xml
00864
00865 def __str__(self):
00866 s = ''
00867 if self.samples != None:
00868 s += "Samples:\n"
00869 s += reindent(str(self.samples),1) + '\n'
00870 if self.resolution != None:
00871 s += "Resolution:\n"
00872 s += reindent(str(self.resolution),1) + '\n'
00873 if self.min_angle != None:
00874 s += "Min_angle:\n"
00875 s += reindent(str(self.min_angle),1) + '\n'
00876 if self.max_angle != None:
00877 s += "Max_angle:\n"
00878 s += reindent(str(self.max_angle),1) + '\n'
00879 return s
00880
00881