def show_trajectory(map_file, log_file):
#load the map and plot it
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(map_file)
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
#initialize all this junk
L, X, Y = load_logfile(log_file)
pltypes = ['o', '^', '<', '>', 's', 'd', 'p', 'h', 'x', 'o']
plcolors = ['r', 'g', 'b', 'm', 'k', 'y', 'c', 'r', 'g', 'b']
#plot the trajectory
XHash = {}
YHash = {}
for i in range(len(L)):
try:
XHash[L[i]].append(X[i])
YHash[L[i]].append(Y[i])
except (KeyError):
XHash[L[i]] = []
YHash[L[i]] = []
for key in XHash.keys():
plot(XHash[key], YHash[key], plcolors[int(key)] + pltypes[int(key)])
plot([X[0]], [Y[0]], 'go')
plot([X[len(X) - 1]], [Y[len(Y) - 1]], 'ro')
show()
def scale_annotations(self, new_mapfilename):
currmap = tklib_log_gridmap()
currmap.load_carmen_map(new_mapfilename)
print "1:", currmap.resolution
print "2:", self.get_map().resolution
sf = self.get_map().resolution / currmap.resolution
print "sf:", sf
newpolygons = []
newpoints = []
for pt in self.points:
newpt = deepcopy(pt)
newpt.x = newpt.x * sf
newpt.y = newpt.y * sf
newpoints.append(newpt)
for poly in self.polygons:
newpoly = deepcopy(poly)
newpoly.X = array(newpoly.X) * sf
newpoly.Y = array(newpoly.Y) * sf
newpoly.X = newpoly.X.tolist()
newpoly.Y = newpoly.Y.tolist()
newpolygons.append(newpoly)
return newpoints, newpolygons
def get_map(self):
if (self.map == None):
if not os.path.exists(self.map_filename):
raise ValueError("map file " + ` self.map_filename ` +
" doesn't exist!")
self.map = tklib_log_gridmap()
self.map.load_carmen_map(self.map_filename)
self.carmen_map = self.map.to_probability_map_carmen()
return self.map
def clog2logfile(map_file, log_file, outfile):
#load the map and plot it
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(map_file)
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
#initialize all this junk
vals = load_carmen_logfile(log_file)
front_readings, rear_readings, true_positions = vals
X, Y = [], []
print "getting the poses"
#append the poses
#tout = open("tmpf", 'w')
i = 0
for pose in true_positions:
#c = mod(i, 10);
#print pose
X.append(pose.x)
Y.append(pose.y)
#tout.write(str(c) + " " + str(pose.x) + " " + str(pose.y) + "\n")
i += 1
#tout.close()
print "ray tracing"
#do the ray_tracing
angles = array(arange(0, 2 * pi, pi / 360.0))
D = []
for i in range(len(X)):
dists = gridmap.ray_trace(X[i], Y[i], angles)
D.append(dists)
#write to a logfile
print "writing the logfile"
write_logfile(X, Y, D, outfile)
print "plotting the trajectory"
#plot the trajectory
plot(X, Y, 'k-')
plot([X[0]], [Y[0]], 'go')
plot([X[len(X) - 1]], [Y[len(Y) - 1]], 'ro')
show()
def logfile2truepose(map_file,
log_file,
x_offset_st=0,
y_offset_st=0,
x_offset_end=0,
y_offset_end=0):
#load the map and plot it
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(map_file)
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
#initialize all this junk
vals = load_carmen_logfile(log_file)
front_readings, rear_readings, true_positions = vals
X, Y = [], []
for pose in true_positions:
X.append(pose.x)
Y.append(pose.y)
#plot the trajectory
plot(X, Y, 'k-')
#plot the start location
plot([X[0]], [Y[0]], 'go')
print "x,y offsets", x_offset_st, y_offset_st
text(X[0] + x_offset_st, Y[0] + y_offset_st, "start", color='w')
#plot the end location
plot([X[len(X) - 1]], [Y[len(Y) - 1]], 'ro')
print "x,y offsets", x_offset_end, y_offset_end
text(X[len(X) - 1] + x_offset_end,
Y[len(Y) - 1] + y_offset_end,
"end",
color='w')
show()
metavar="FILE")
parser.add_option("-t",
"--topology",
dest="topology",
help="Topology",
metavar="FILE")
(options, args) = parser.parse_args()
topology = cPickle.load(open(options.topology, 'r'))
print "getting topological map"
#ion()
figure()
cmap = tklib_log_gridmap()
cmap.load_carmen_map(options.carmen_map)
plot_map(cmap.to_probability_map_carmen(), cmap.x_size, cmap.y_size)
XY = transpose([node.xy for node in topology.nodes])
plot(XY[0], XY[1], 'ko')
for edge in topology.edges:
plot(edge.path[0], edge.path[1])
#axis([36.0,38.0, 45, 47])
#print edge.start_i
#draw()
#if(edge.path[0][0] < 38 and edge.path[0][0] > 36 and
# edge.path[1][0] < 47 and edge.path[1][0] > 45):
# raw_input("enter")