def read_materials(self, phantfile):
"""Reads material name list from given file object"""
try:
# number of materials
self.nmaterials = scanf.fscanf(phantfile, "%d")[0]
self.debug_print(self.nmaterials)
# list of materials
self.materials = []
for i in range(self.nmaterials):
material = scanf.fscanf(phantfile, "%s")[0]
self.materials.append(material)
self.debug_print(self.materials)
# dummy estepe values, one per material
# even though they're dummy, I've seen different
# values (0.0, and 1.0), so might as well store them
self.estepe = []
for i in range(self.nmaterials):
self.estepe.append(scanf.fscanf(phantfile, "%f"))
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def read_materials(self, phantfile):
"""Reads material name list from given file object"""
try:
# number of materials
self.nmaterials = scanf.fscanf(phantfile, "%d")[0]
self.debug_print(self.nmaterials)
# list of materials
self.materials = []
for i in range(self.nmaterials):
material = scanf.fscanf(phantfile, "%s")[0]
self.materials.append(material)
self.debug_print(self.materials)
# dummy estepe values, one per material
# even though they're dummy, I've seen different
# values (0.0, and 1.0), so might as well store them
self.estepe = []
for i in range(self.nmaterials):
self.estepe.append(scanf.fscanf(phantfile, "%f"))
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def read_materialscan(self, phantfile):
"""Read in the material scan from given file object"""
# NB: a 3D array is made up of a stack of slices, each
# slice being a matrix
# the index order is z,y,x where x is column index,
# y is row index, and z is slice index
try:
# scan of materials
self.materialscan = zeros([self.dimensions[2], self.dimensions[1], self.dimensions[0]], type=Int32)
for k in range(self.dimensions[2]):
for j in range(self.dimensions[1]):
print "k = ", k, ", ", "j = ", j
matrow = []
matrow.extend(scanf.fscanf(phantfile, "%s ")[0])
if len(matrow) != self.dimensions[0]:
print matrow, len(matrow)
for i in range(len(matrow)):
self.materialscan[k, j, i] = int(matrow[i])
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def read_densityscan(self, phantfile):
"""Read in the density scan from given file object"""
# NB: a 3D array is made up of a stack of slices, each
# slice being a matrix
# the index order is z,y,x where x is column index,
# y is row index, and z is slice index
try:
# scan of densities
self.densityscan = zeros([self.dimensions[2],self.dimensions[1],
self.dimensions[0]], type=Float32)
formatstr = ""
for i in range(self.dimensions[0]):
formatstr = formatstr + "%f "
for k in range(self.dimensions[2]):
for j in range(self.dimensions[1]):
self.densityscan[k,j,:] = array(scanf.fscanf(phantfile, formatstr))
except IOError:
raise IOError
except scanf.FormatError:
print "FOOBAR"
raise scanf.FormatError
def read_materialscan(self, phantfile):
"""Read in the material scan from given file object"""
# NB: a 3D array is made up of a stack of slices, each
# slice being a matrix
# the index order is z,y,x where x is column index,
# y is row index, and z is slice index
try:
# scan of materials
self.materialscan = zeros([self.dimensions[2],self.dimensions[1],
self.dimensions[0]], type=Int32)
for k in range(self.dimensions[2]):
for j in range(self.dimensions[1]):
print "k = ", k, ", ", "j = ", j
matrow = []
matrow.extend(scanf.fscanf(phantfile, "%s ")[0])
if len(matrow) != self.dimensions[0]:
print matrow, len(matrow)
for i in range(len(matrow)):
self.materialscan[k,j,i] = int(matrow[i])
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def __read_densityscan(self, phantfile):
"""Read in the density scan from given file object"""
# NB: a 3D array is made up of a stack of slices, each
# slice being a matrix
# the index order is z,y,x where x is column index,
# y is row index, and z is slice index
try:
# scan of densities
self.densityscan = np.zeros((self.dimensions[2],self.dimensions[1],
self.dimensions[0]), order='C',
dtype=float)
formatstr = ""
for i in range(self.dimensions[0]):
formatstr = formatstr + "%f "
for k in range(self.dimensions[2]):
for j in range(self.dimensions[1]):
self.densityscan[k,j,:] = np.array(scanf.fscanf(phantfile, formatstr))
except IOError:
raise IOError
except scanf.FormatError:
print "FOOBAR"
raise scanf.FormatError
def __read_materialscan(self, phantfile):
"""Read in the material scan from given file object"""
# NB: a 3D array is made up of a stack of slices, each
# slice being a matrix
# the index order is z,y,x where x is column index,
# y is row index, and z is slice index
# Ordering in the file:
# x increases to the right
# y increases downward
# z increases downward
try:
# scan of materials
self.materialscan = np.zeros((self.dimensions[2],
self.dimensions[1],
self.dimensions[0]),
order='C', dtype=int)
for k in range(self.dimensions[2]):
for j in range(self.dimensions[1]):
matrow = []
matrow.extend(scanf.fscanf(phantfile, "%s ")[0])
for i in range(len(matrow)):
self.materialscan[k,j,i] = int(matrow[i])
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def __read_voxels(self, phantfile):
"""Read voxel data from given file object"""
try:
# dimensions
self.dimensions = scanf.fscanf(phantfile, "%d %d %d")
self.xedges = np.zeros(self.dimensions[0]+1, order='C', dtype=float)
self.yedges = np.zeros(self.dimensions[1]+1, order='C', dtype=float)
self.zedges = np.zeros(self.dimensions[2]+1, order='C', dtype=float)
# x-edges
formatstr = ""
for i in range(self.dimensions[0]+1):
formatstr = formatstr + "%f "
self.xedges = np.array(scanf.fscanf(phantfile, formatstr),
dtype=float)
self.__debug_print(self.xedges)
# y-edges
formatstr = ""
for i in range(self.dimensions[1]+1):
formatstr = formatstr + "%f "
self.yedges = np.array(scanf.fscanf(phantfile, formatstr),
dtype=float)
self.__debug_print(self.yedges)
# z-edges
formatstr = ""
for i in range(self.dimensions[2]+1):
formatstr = formatstr + "%f "
self.zedges = np.array(scanf.fscanf(phantfile, formatstr),
dtype=float)
self.__debug_print(self.zedges)
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def read_voxels(self, phantfile):
"""Read voxel data from given file object"""
try:
# dimensions
self.dimensions = scanf.fscanf(phantfile, "%d %d %d")
self.debug_print(("dimensions = ", self.dimensions))
self.xedges = zeros(self.dimensions[0] + 1, type=Float32)
self.yedges = zeros(self.dimensions[1] + 1, type=Float32)
self.zedges = zeros(self.dimensions[2] + 1, type=Float32)
# x-edges
formatstr = ""
for i in range(self.dimensions[0] + 1):
formatstr = formatstr + "%f "
self.xedges = array(scanf.fscanf(phantfile, formatstr))
self.debug_print(self.xedges)
# y-edges
formatstr = ""
for i in range(self.dimensions[1] + 1):
formatstr = formatstr + "%f "
self.yedges = array(scanf.fscanf(phantfile, formatstr))
self.debug_print(self.yedges)
# z-edges
formatstr = ""
for i in range(self.dimensions[2] + 1):
formatstr = formatstr + "%f "
self.zedges = array(scanf.fscanf(phantfile, formatstr))
self.debug_print(self.zedges)
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def read_voxels(self, phantfile):
"""Read voxel data from given file object"""
try:
# dimensions
self.dimensions = scanf.fscanf(phantfile, "%d %d %d")
self.debug_print(("dimensions = ", self.dimensions))
self.xedges = zeros(self.dimensions[0]+1, type=Float32)
self.yedges = zeros(self.dimensions[1]+1, type=Float32)
self.zedges = zeros(self.dimensions[2]+1, type=Float32)
# x-edges
formatstr = ""
for i in range(self.dimensions[0]+1):
formatstr = formatstr + "%f "
self.xedges = array(scanf.fscanf(phantfile, formatstr))
self.debug_print(self.xedges)
# y-edges
formatstr = ""
for i in range(self.dimensions[1]+1):
formatstr = formatstr + "%f "
self.yedges = array(scanf.fscanf(phantfile, formatstr))
self.debug_print(self.yedges)
# z-edges
formatstr = ""
for i in range(self.dimensions[2]+1):
formatstr = formatstr + "%f "
self.zedges = array(scanf.fscanf(phantfile, formatstr))
self.debug_print(self.zedges)
except IOError:
raise IOError
except scanf.FormatError:
raise scanf.FormatError
def run(self):
# 读取.graph文件，初始化地图中节点的数目dimension
graph_file = open(mapGraph_filename, "r")
read_tuple = scanf.fscanf(graph_file, "%d %d %d %f")
global dimension
global width_px
global height_px
global resolution
dimension = read_tuple[0]
width_px = read_tuple[1]
height_px = read_tuple[2]
resolution = read_tuple[3]
graph_file.close()
# 目前，对graph文件后面的各节点信息并未加以利用
# 打开记录机器人路径的文件
global robotpath_filename
self.robotpath_file = open(robotpath_filename, "w")
# lastVisitList 等全局变量的初始化在 MyApp 中的 send_start 函数中执行
lc = lcm.LCM()
subscription_status = lc.subscribe("ROBOTSTATUS", self.my_handler_status)
subscription_idleness = lc.subscribe("VERTEXIDLENESS", self.my_handler_idleness)
try:
timeout = 0.5 # amount of time to wait, in seconds
while True:
rfds, wfds, efds = select.select([lc.fileno()], [], [], timeout)
if rfds:
lc.handle()
else:
if self._stop.isSet():
break
except KeyboardInterrupt:
pass
lc.unsubscribe(subscription_status)
lc.unsubscribe(subscription_idleness)
# 关闭记录机器人路径的文件
self.robotpath_file.close()
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from PIL import Image, ImageDraw
import scanf
# 需要读取的 .graph 和 .ppm 文件的名称
name_only = "maps/grid/grid"
# 保存机器人路径（由一系列位置点组成）的文件名称
robotpath_filename = "results/grid_4_robotpath.txt"
# 读取.graph文件，初始化地图中节点的数目dimension
graph_file = open( name_only + '.graph', 'r' )
read_tuple = scanf.fscanf(graph_file, "%d %d %d %f")
dimension = read_tuple[0]
width_px = read_tuple[1]
height_px = read_tuple[2]
resolution = read_tuple[3]
graph_file.close()
mapImage = Image.open( name_only + '_info.ppm' )
draw = ImageDraw.Draw(mapImage) # 定义画图对象
robotpath_file = open(robotpath_filename, 'r')
for line in robotpath_file:
line = line.rstrip() # remove end-of-line
if len(line) == 0:
continue # 跳过空行
parts = line.split(' ')
def __init__(self, master):
self.root = master
textLabel = Tkinter.Label(master, text='Topological Map')
textLabel.pack()
imageFrame = Tkinter.Frame(master, relief=Tkinter.RAISED, borderwidth=1)
imageFrame.pack(side=Tkinter.TOP)
global mapImage_filename
self.mapImage = Image.open( mapImage_filename )
mapPhoto = ImageTk.PhotoImage(self.mapImage)
self.imageLabel = Tkinter.Label(imageFrame, image=mapPhoto)
self.imageLabel.image = mapPhoto # keep a reference!
self.imageLabel.pack()
buttonFrame = Tkinter.Frame(master)
buttonFrame.pack(side=Tkinter.BOTTOM)
self.startButton = Tkinter.Button(buttonFrame, text="Start", command=self.send_start)
self.startButton.pack(side=Tkinter.LEFT, padx=10, pady=10)
self.stopButton = Tkinter.Button(buttonFrame, text="Stop", command=self.send_stop)
self.stopButton.pack(side=Tkinter.RIGHT, padx=10, pady=10)
# 读取.graph文件，初始化地图中节点的数目dimension
global mapGraph_filename
graph_file = open( mapGraph_filename, 'r' )
read_tuple = scanf.fscanf(graph_file, "%d %d %d %f")
global dimension
global width_px
global height_px
global resolution
dimension = read_tuple[0]
width_px = read_tuple[1]
height_px = read_tuple[2]
resolution = read_tuple[3]
graph_file.close()
# 目前，对graph文件后面的各节点信息并未加以利用
# 打开记录机器人路径的文件
global robotpath_filename
self.robotpath_file = open(robotpath_filename, 'r')
mapImage = self.mapImage.copy() # 复制图像，类的成员变量self.mapImage存储着地图，将机器人位置用圆形表示画在地图上
draw = ImageDraw.Draw(mapImage) # 定义画图对象
for i in range(0,4):
line = self.robotpath_file.readline()
line = line.rstrip() # remove end-of-line
parts = line.split(' ')
that_time = float(parts[0])
robot_id = int(parts[1])
x_robot = float(parts[2])
y_robot = float(parts[3])
th_robot = float(parts[4])
# 设置机器人占据的区域，以供画图
x_robot_px = int(x_robot/resolution)
y_robot_px = int(height_px-y_robot/resolution)
robot_radius = 3 # 画图时的机器人半径，单位像素pixel
box = ( x_robot_px-robot_radius, y_robot_px-robot_radius, x_robot_px+robot_radius, y_robot_px+robot_radius )
if robot_id == 0:
# blue
draw.ellipse( box, outline=(0,0,255), fill=(0,0,255) ) # 将机器人位置用圆形表示画在地图上
elif robot_id == 1:
# red
draw.ellipse( box, outline=(255,0,0), fill=(255,0,0) ) # 将机器人位置用圆形表示画在地图上
elif robot_id == 2:
# green
draw.ellipse( box, outline=(0,255,0), fill=(0,255,0) ) # 将机器人位置用圆形表示画在地图上
else:
# magenta
draw.ellipse( box, outline=(255,0,255), fill=(255,0,255) ) # 将机器人位置用圆形表示画在地图上
del draw # 释放画图对象所占内存
mapPhoto = ImageTk.PhotoImage(mapImage)
self.imageLabel.configure(image=mapPhoto) # 更新Label显示的图像
self.imageLabel.image = mapPhoto # keep a reference!
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from PIL import Image, ImageDraw
import scanf
# 需要读取的 .graph 和 .ppm 文件的名称
name_only = "maps/grid/grid"
# 保存机器人路径（由一系列位置点组成）的文件名称
robotpath_filename = "results/grid_4_robotpath.txt"
# 读取.graph文件，初始化地图中节点的数目dimension
graph_file = open(name_only + ".graph", "r")
read_tuple = scanf.fscanf(graph_file, "%d %d %d %f")
dimension = read_tuple[0]
width_px = read_tuple[1]
height_px = read_tuple[2]
resolution = read_tuple[3]
graph_file.close()
mapImage = Image.open(name_only + "_info.ppm")
draw = ImageDraw.Draw(mapImage) # 定义画图对象
robotpath_file = open(robotpath_filename, "r")
for line in robotpath_file:
line = line.rstrip() # remove end-of-line
if len(line) == 0:
continue # 跳过空行
parts = line.split(" ")
def __init__(self, master):
self.root = master
textLabel = Tkinter.Label(master, text='Topological Map')
textLabel.pack()
imageFrame = Tkinter.Frame(master,
relief=Tkinter.RAISED,
borderwidth=1)
imageFrame.pack(side=Tkinter.TOP)
global mapImage_filename
self.mapImage = Image.open(mapImage_filename)
mapPhoto = ImageTk.PhotoImage(self.mapImage)
self.imageLabel = Tkinter.Label(imageFrame, image=mapPhoto)
self.imageLabel.image = mapPhoto # keep a reference!
self.imageLabel.pack()
buttonFrame = Tkinter.Frame(master)
buttonFrame.pack(side=Tkinter.BOTTOM)
self.startButton = Tkinter.Button(buttonFrame,
text="Start",
command=self.send_start)
self.startButton.pack(side=Tkinter.LEFT, padx=10, pady=10)
self.stopButton = Tkinter.Button(buttonFrame,
text="Stop",
command=self.send_stop)
self.stopButton.pack(side=Tkinter.RIGHT, padx=10, pady=10)
# 读取.graph文件，初始化地图中节点的数目dimension
global mapGraph_filename
graph_file = open(mapGraph_filename, 'r')
read_tuple = scanf.fscanf(graph_file, "%d %d %d %f")
global dimension
global width_px
global height_px
global resolution
dimension = read_tuple[0]
width_px = read_tuple[1]
height_px = read_tuple[2]
resolution = read_tuple[3]
graph_file.close()
# 目前，对graph文件后面的各节点信息并未加以利用
# 打开记录机器人路径的文件
global robotpath_filename
self.robotpath_file = open(robotpath_filename, 'r')
mapImage = self.mapImage.copy(
) # 复制图像，类的成员变量self.mapImage存储着地图，将机器人位置用圆形表示画在地图上
draw = ImageDraw.Draw(mapImage) # 定义画图对象
for i in range(0, 4):
line = self.robotpath_file.readline()
line = line.rstrip() # remove end-of-line
parts = line.split(' ')
that_time = float(parts[0])
robot_id = int(parts[1])
x_robot = float(parts[2])
y_robot = float(parts[3])
th_robot = float(parts[4])
# 设置机器人占据的区域，以供画图
x_robot_px = int(x_robot / resolution)
y_robot_px = int(height_px - y_robot / resolution)
robot_radius = 3 # 画图时的机器人半径，单位像素pixel
box = (x_robot_px - robot_radius, y_robot_px - robot_radius,
x_robot_px + robot_radius, y_robot_px + robot_radius)
if robot_id == 0:
# blue
draw.ellipse(box, outline=(0, 0, 255),
fill=(0, 0, 255)) # 将机器人位置用圆形表示画在地图上
elif robot_id == 1:
# red
draw.ellipse(box, outline=(255, 0, 0),
fill=(255, 0, 0)) # 将机器人位置用圆形表示画在地图上
elif robot_id == 2:
# green
draw.ellipse(box, outline=(0, 255, 0),
fill=(0, 255, 0)) # 将机器人位置用圆形表示画在地图上
else:
# magenta
draw.ellipse(box, outline=(255, 0, 255),
fill=(255, 0, 255)) # 将机器人位置用圆形表示画在地图上
del draw # 释放画图对象所占内存
mapPhoto = ImageTk.PhotoImage(mapImage)
self.imageLabel.configure(image=mapPhoto) # 更新Label显示的图像
self.imageLabel.image = mapPhoto # keep a reference!
