Пример #1
0
def get_obstacles(file):
    """
    get obstacle's coordinates from world file

    file: the world file
    """
    xml = XMLReader(file, 'parameters')

    vertexs_list = []

    for item in xml.read():
        points = []
        temp = []
        if item[0] == 'obstacle':
            pose = item[1][0]
            geometry = item[1][1]
            y, x, theta = pose[1][0][1], pose[1][1][1], pose[1][2][1]
            for point in geometry[1]:
                point_y, point_x = point[1][0][1], point[1][1][1]
                points.append([point_x, point_y])
            origin = array([x, y])
            origin = origin.reshape(2, 1)
            A = array([
                [cos(theta), -sin(theta)],  # rotation matrix
                [sin(theta), cos(theta)]
            ])
            #print A.shape, 'A'

            for x0 in points:
                x0 = array(x0)
                #print x0.shape
                x0 = x0.reshape(2, 1)
                #print origin
                vertexs = (dot(A, x0) + origin)
                #print vertexs, round(vertexs[0],1),'\n'
                temp.append((round(vertexs[0], 1), round(vertexs[1], 1)))
            #print temp, 'temp'
            vertexs_list.append(temp)

    return vertexs_list


# pose is the bottom left corner
# origin = origin.reshape(2, 1)
#    x0 = x0.reshape(2, 1)
#    x0 = x0 - origin # assingment operator (-=) would modify original x0

#    A = array([[cos(theta), -sin(theta)], # rotation matrix
#               [sin(theta), cos(theta)]])

#    return (dot(A, x0) + origin).ravel()

#    return (dot(A, x0) + origin).ravel()
Пример #2
0
 def load_click(self):
     filename = QtGui.QFileDialog.getOpenFileName(self,
                     "Select a file with parameters",
                     "supervisors",
                     "XML files (*.xml)")
     if filename is not None:
         reader = XMLReader(filename, 'parameters')
         cache = self.contents.get_xmlstruct()
         try:
             self.contents.use_xmlstruct(reader.read())
         except Exception as e:
             QtGui.QMessageBox.critical(self,"Loading parameters failed",str(e))
             self.contents.use_xmlstruct(cache)
def get_obstacles(file):
    """
    get obstacle's coordinates from world file

    file: the world file
    """
    xml = XMLReader(file, 'parameters')
    
    vertexs_list = []
    
    for item in xml.read():
        points = []
        temp = []
        if item[0] == 'obstacle':
            pose = item[1][0]
            geometry = item[1][1]
            y, x, theta = pose[1][0][1], pose[1][1][1], pose[1][2][1]
            for point in geometry[1]:
                point_y, point_x = point[1][0][1], point[1][1][1]
                points.append([point_x, point_y])
            origin = array([x,y])
            origin = origin.reshape(2, 1)
            A = array([[cos(theta), -sin(theta)], # rotation matrix
                      [sin(theta), cos(theta)]])
            #print A.shape, 'A'
            
            for x0 in points:
                x0 = array(x0)
                #print x0.shape
                x0 = x0.reshape(2,1)
                #print origin
                vertexs = (dot(A, x0) + origin)
                #print vertexs, round(vertexs[0],1),'\n'
                temp.append((round(vertexs[0],1), round(vertexs[1],1)))
            #print temp, 'temp'
            vertexs_list.append(temp)

    return vertexs_list
# pose is the bottom left corner
     # origin = origin.reshape(2, 1)
     #    x0 = x0.reshape(2, 1)
     #    x0 = x0 - origin # assingment operator (-=) would modify original x0

     #    A = array([[cos(theta), -sin(theta)], # rotation matrix
     #               [sin(theta), cos(theta)]])

     #    return (dot(A, x0) + origin).ravel()

     #    return (dot(A, x0) + origin).ravel()
    def load_click(self):
        filename = QtGui.QFileDialog.getOpenFileName(self,
                        "Select a file with parameters",
                        "supervisors",
                        "XML files (*.xml)")
        if filename is not None:
            reader = XMLReader(filename, 'parameters')
            cache = self.contents.get_xmlstruct()
            try:
                self.contents.use_xmlstruct(reader.read())
            except Exception as e:

                #QtGui.QMessageBox.critical(self,"Loading parameters failed",str(e))
                QtGui.QMessageBox.critical(self,"Loading parameters failed","\n".join(format_exception(*sys.exc_info())))
                self.contents.use_xmlstruct(cache)