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))
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)
