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


naoqi_tools
Author(s): Mikael Arguedas
autogenerated on Wed Aug 16 2017 02:28:16