prop.py

Go to the documentation of this file.

import ConfigParser, os
import rospy

class Marker():
    def __init__(self):
        self.name = ''
        self.position = [0,0,0]
    def __str__(self):
        return "%s: %f %f %f"%(self.name, self.position[0], self.position[1],
                              self.position[2])

class Prop():
    def __init__(self):
        self.Name=''
        self.ExtraStretch=0.1
        self.PositionOffset=[0, 0, 0]
        self.RotationOrder='ZYX'
        self.OrientationOffset = [0, 0, 0]
        self.Markers=[]

    def __str__(self):
        str = ''
        str = str + ("[%s]\n"%(self.Name))
        #print "PositionOffset: %f %f"%([0,1])
        #print ["{0:f}".format(i) for i in self.PositionOffset])
        str =  str + ("\n".join("%s"%(x) for x in self.Markers))
        return str
        # print "RotationOrder: %f %f %f"%(self.PositionOffset[:])
        # print "OrientationOffset: %f %f %f"%(self.PositionOffset)
        # print "ExtraStretch: %f"%(self.PositionOffset)
        # print "Number of markers: %i"%(self.PositionOffset)

    def parse_prop_file(self,file):
        config = ConfigParser.ConfigParser()
        config.readfp(file)
        self.Name = config.get('RigidBody','Name',1)
        if self.Name == '':
            return False
        self.ExtraStretch = config.getfloat('RigidBody','ExtraStretch')
        self.RotationOrder = config.get('RigidBody','RotationOrder')
        # now the more difficult stuff
        pos_offset = config.get('RigidBody','PositionOffset')
        self.PositionOffset = [float(x) for x in eval(pos_offset)]
        orientation_offset = config.get('RigidBody','OrientationOffset')
        self.OrientationOffset = [float(x) for x in eval(orientation_offset)]
        markers = config.get('RigidBody','NumberOfMarkers')
        m = markers.split('\n')
        number_of_markers = int(m[0])
        for x in m[1:]:
            line = x.split(',')
            marker = Marker()
            elements = [s.strip() for s in line]
            if len(elements) != 6:
                rospy.logerr( "Parsing of Markers failed!" )
                return False
            marker.name = elements[4]
            marker.position[0] = float(elements[0])/1000
            marker.position[1] = float(elements[1])/1000
            marker.position[2] = float(elements[2])/1000
            self.Markers.append(marker)
            # TODO: what is the remainder?
        rospy.loginfo("parsing of %s done"%(self.Name))
        return True