def drawMap(self):
gridmap = self.tagFile.map
carmen_maptools.plot_map(self.tagFile.carmen_map,
gridmap.x_size,
gridmap.y_size,
cmap="carmen_cmap_white",
curraxis=self.axes)
def test1(argv):
filename = argv[1]
num_samples = int(argv[2])
path_planner = carmen_path_planner(filename, num_samples)
#plot(path_planner.locations[0], path_planner.locations[1], 'ro')
#mymap = path_planner.map.to_probability_map_carmen()
#plot_map(mymap, path_planner.map.get_y_size(),path_planner.map.get_x_size())
#show()
pts = path_planner.get_loop_pts()
if (pts != None):
plot(pts[0], pts[1], 'r-')
mymap = path_planner.map.to_probability_map_carmen()
plot_map(mymap, path_planner.map.get_y_size(),
path_planner.map.get_x_size())
plot([path_planner.locations[0, path_planner.s_ind]],
[path_planner.locations[1, path_planner.s_ind]], 'ro')
plot([path_planner.locations[0, path_planner.e_ind]],
[path_planner.locations[1, path_planner.e_ind]], 'rx')
for i in range(len(path_planner.sentinels)):
plot([path_planner.locations[0, path_planner.sentinels[i]]],
[path_planner.locations[1, path_planner.sentinels[i]]], 'r^')
show()
def test1(filename, mfilter_width, num_iterations):
tklib_skeleton = carmen_map_skeletonizer(filename, mfilter_width,
num_iterations)
curr_map = tklib_skeleton.get_skeleton()
imshow(curr_map, origin=1, cmap=cm.gray)
title("skeleton with staircases removed")
#show the control point map
figure()
junction_map, Ij = find_junction_points(curr_map)
imshow(junction_map, origin=1, cmap=cm.gray)
title("junction points")
figure()
end_map, Ie = find_end_points(curr_map)
imshow(end_map, origin=1, cmap=cm.gray)
title("end points")
figure()
themap = tklib_skeleton.gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, tklib_skeleton.gridmap.x_size,
tklib_skeleton.gridmap.y_size)
XYj = ind_to_xy(tklib_skeleton.gridmap, Ij)
XYe = ind_to_xy(tklib_skeleton.gridmap, Ie)
pj, = plot(XYj[0], XYj[1], 'bo')
pe, = plot(XYe[0], XYe[1], 'g^')
legend((pj, pe), ("Junction Points", "End Points"), shadow=True)
show()
def test1():
gridmap = tklib_log_gridmap()
print "loading thickwean"
gridmap.load_carmen_map("/home/tkollar/installs/carmen/data/thickwean.map")
loc = gridmap.get_random_open_location(0.2)
plot([loc[0]], [loc[1]], 'b^')
locations = []
for i in range(100):
print i
open_loc = gridmap.get_random_open_location(
0.2) #_location_from_pt(loc, 2.0, 0.2);
locations.append(open_loc)
locations = transpose(locations)
X, Y = locations
plot(X, Y, 'ro')
themap = gridmap.to_probability_map_carmen()
#print themap
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
show()
def plot_map(gridmap):
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap,
gridmap.x_size,
gridmap.y_size,
cmap="carmen_cmap_white")
mpl.draw()
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 view_logfile(filename, logfile, x_offset=0, y_offset=0):
gridmap = tklib_gridmap()
gridmap.load_carmen_map(filename)
themap = gridmap.get_map()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
freadings, rreadings, tp, odom = load_carmen_logfile(logfile)
print "len(freadings)", freadings
#for elt in freadings:
# print elt
#x_offset = 0#freadings[0].location.x - 19.25
#y_offset = 0#freadings[0].location.y - 50.75
X, Y = [], []
myfile = open('test_out.log', 'w')
for elt in freadings:
pt = elt.location
X.append(pt.x - x_offset)
Y.append(pt.y - y_offset)
elt.location.x = elt.location.x - x_offset
elt.location.y = elt.location.y - y_offset
myfile.write(elt.carmen_str() + "\n")
myfile.close()
plot(X, Y, 'r-')
show()
def test1():
ion()
#class noise_model2D{
tv_nm = noise_model2D(1.0, 0, 0.1, 0.0)
rv_nm = noise_model2D(0, 1.0, 0.0, 0.1)
slip_nm = noise_model2D(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.1, 0.01, 0.0001)
x, y, theta = [6, 6, 0]
#plot the measurements
reading_plt, = plot([], [], 'ro');
#true plots
position_plt, = plot([], [], 'go');
orient_plt, = plot([], []);
#corrupted plots
positionc_plt, = plot([], [], 'ko');
orientc_plt, = plot([], []);
hurdles_plt, = plot([], [], 'k^', markersize=10);
#make a simulator, load and ploxt the map
#mysim = simulator2D(array([x,y,theta]), nm, onm, "/home/tkollar/local/tklib/data/maps/hurdles.map.gz");
mysim = simulator2D(nm, onm, "../../data/maps/longwood.map");
mysim.add_hurdles(0.2413, 0.03, 20)
#"/home/tkollar/local/tklib/data/maps/hurdles.map.gz");
themap = mysim.map.get_map()
carmen_maptools.plot_map(themap, mysim.map.x_size, mysim.map.y_size);
#do some simulation
for i in range(300):
mysim.simulate_motion(1.0, 0.6, 0.1);
sim_meas = mysim.simulate_measurements();
x, y, theta = mysim.get_true_pose()
xc, yc, thetac = mysim.get_pose()
if(len(sim_meas) > 0):
XY = carmen_util_reading_to_xy(mysim.get_true_pose(), sim_meas);
XYc = carmen_util_reading_to_xy(mysim.get_pose(), sim_meas);
#do some plotting
reading_plt.set_data(XY[0], XY[1]);
position_plt.set_data([x], [y]);
orient_plt.set_data([x, x+cos(theta)], [y, y+sin(theta)]);
positionc_plt.set_data([xc], [yc]);
orientc_plt.set_data([xc, xc+cos(thetac)], [yc, yc+sin(thetac)]);
hurdles = mysim.simulate_extracted_hurdles(mysim.get_true_pose())
if(len(hurdles) > 0):
hurdles_plt.set_data(hurdles[0], hurdles[1])
draw()
def makeBackground(tagFile):
gridmap = tagFile.map
print "x, y", gridmap.x_size, gridmap.y_size
#figure = mpl.figure(figsize=(10,10), dpi=600, frameon=False)
figure = mpl.figure(frameon=False)
axes = figure.add_axes([0, 0, 1, 1], frameon=False)
carmen_maptools.plot_map(tagFile.carmen_map,
gridmap.x_size, gridmap.y_size,
cmap="carmen_cmap_white",
curraxis=axes)
from landmarkSelectorTableModel import plotLandmarks
plotLandmarks(figure, tagFile, useText=False, washoutFactor=0.4)
mpl.subplots_adjust(left=0, right=1, top=1, bottom=0, wspace=0, hspace=0)
axes.axis([0, gridmap.x_size, 0, gridmap.y_size])
def save():
axes.set_axis_off()
fname = "%s/data/directions/stata3_aaai/stata3_aaai.png" % TKLIB_HOME
print "saving", fname
mpl.savefig(fname, dpi=200)
save()
def partition_view(clusters):
#load the map
gridmap = clusters.get_map()
themap = gridmap.to_probability_map_carmen()
for i in range(clusters.numkmeans):
#ion()
figure()
title(str(i))
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
#temporary
#clusters.gridcell_skip = 25.0
tmap, tmap_cnt, tmap_locs = clusters.get_topological_map(i)
newtmap = {}
for elt in tmap.keys():
newtmap[str(elt)] = array(tmap[elt])
newtmap_locs = {}
for elt in tmap_locs.keys():
newtmap_locs[str(elt)] = array(tmap_locs[elt])
print newtmap_locs
savemat('tmap.mat', newtmap)
savemat('tmap_locs.mat', newtmap_locs)
X, Y = [], []
for loc_st_i in tmap_locs.keys():
for loc_end_i in tmap[loc_st_i]:
if (loc_st_i == -1 or loc_end_i == -1):
continue
xy_st = tmap_locs[loc_st_i]
xy_end = tmap_locs[loc_end_i]
#vtags, itags = clusters.tf.compute_visibility_tag(xy_st[0],
# xy_st[1])
#print vtags
#vtags, itags = clusters.tf.compute_visibility_tag(xy_end[0],
# xy_end[1])
#print vtags
plot([xy_st[0], xy_end[0]], [xy_st[1], xy_end[1]], 'ro-')
#draw()
#raw_input()
figure()
title(str(i))
show_explosion(clusters)
figure()
title(str(i))
show_partitions(clusters.get_map(), clusters.samples,
clusters.mylabels[i])
figure()
clusters.skel.G = None
topo_map_view_fullpaths(clusters)
show()
def test2():
ion()
#class noise_model2D{
tv_nm = noise_model2D(1.0, 0, 0.1, 0.0)
rv_nm = noise_model2D(0, 1.0, 0.0, 0.1)
slip_nm = noise_model2D(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.1, 0.01, 0.0001)
x, y, theta = [6, 6, 0]
#make a simulator, load and plot the map
#mysim = simulator2D([x,y,theta], nm, onm, "/home/tkollar/local/tklib/data/maps/hurdles.map.gz");
mysim = simulator2D(nm, onm, "/home/tkollar/local/repositories/tklib/data/maps/hurdles.map.gz");
mymap = occupancy_grid_mapper(0.9,0.9, 0.5, 20, 20, 0.1, mysim.get_pose(), 0.17)
#carmen_maptools.plot_map(mysim.map.get_map(), mysim.map.x_size, mysim.map.y_size);
nn_plt, = plot([], [], 'ro');
orig_plt, = plot([], [], 'bx');
icp_plt, = plot([], [], 'gx');
robot_pose_plt, = plot([], [], 'ko');
robot_orient_plt, = plot([], [], 'k');
#do some simulation
for i in range(10):
mysim.simulate_motion(1.0, 0.6, 0.1);
sim_meas = mysim.simulate_measurements();
if(len(sim_meas) > 0):
pts = carmen_util_reading_to_xy(mysim.get_pose(), sim_meas);
X, Y = pts
orig_plt.set_data(X, Y);
print "updating map"
if(len(sim_meas) > 0):
mymap.update_icp(mysim.get_pose(), sim_meas, 1);
X, Y = mymap.icp_map(pts, 1);
icp_plt.set_data(X, Y);
X, Y = mymap.nearest_neighbors_map_alloc(pts);
nn_plt.set_data(X, Y);
x, y, theta = mymap.get_pose()
robot_pose_plt.set_data([x], [y]);
robot_orient_plt.set_data([x, x+0.3*cos(theta)], [y, y+0.3*sin(theta)]);
carmen_maptools.plot_map(mymap.map.to_probability_map(), mymap.map.x_size, mymap.map.y_size, cmap="binary");
draw()
show()
def view_map(filename):
gridmap = tklib_log_gridmap()
#while(1):
gridmap.load_carmen_map(filename)
#del gridmap
themap = gridmap.to_probability_map_carmen()
#carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size, cmap="carmen_cmap_gray");
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
show()
def test2(argv):
filename = argv[1]
num_samples = int(argv[2])
path_planner = carmen_path_planner(filename, num_samples)
pts = path_planner.get_path([45, 25], [45, 45])
if (pts != None):
mymap = path_planner.map.to_probability_map_carmen()
plot_map(mymap, path_planner.map.get_y_size(),
path_planner.map.get_x_size())
plot(pts[0], pts[1], 'r-')
show()
def test1():
ion()
#class noise_model2D{
tv_nm = noise_model2D(1.0, 0, 0.1, 0.0)
rv_nm = noise_model2D(0, 1.0, 0.0, 0.1)
slip_nm = noise_model2D(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.1, 0.01, 0.0001)
x, y, theta = [6, 6, 0]
#make a simulator, load and plot the map
print "loading simulator"
mysim = simulator2D(nm, onm, "/home/tkollar/local/repositories/tklib/data/maps/thickwean.map");
print "starting gridmapper"
mymap = occupancy_grid_mapper(0.9,0.9, 0.5, 20, 20, 0.1, mysim.get_pose(), 0.17)
carmen_maptools.plot_map(mysim.map.get_map(), mysim.map.x_size, mysim.map.y_size);
nn_plt, = plot([], [], 'ro');
orig_plt, = plot([], [], 'bx');
#do some simulation
for i in range(100):
#mysim.simulate_motion(1.0, 0.6, 0.1);
sim_meas = mysim.simulate_measurements();
print "starting"
if(len(sim_meas) > 0):
pts = carmen_util_reading_to_xy(mysim.get_pose(), sim_meas);
#x, y, theta = mysim.get_true_pose()
#xc, yc, thetac = mysim.get_pose()
#print pts
X, Y = pts
orig_plt.set_data(X, Y);
if(i==0):
if(len(sim_meas) > 0):
mymap.update_icp(1.0*0.1, 0.6*0.1, sim_meas);
X, Y = mymap.nearest_neighbors_map_alloc(pts);
nn_plt.set_data(X, Y);
#print X, Y
draw()
#carmen_maptools.plot_map(mymap.map.get_map(), mymap.map.x_size, mymap.map.y_size, cmap="binary");
#savefig("video/"+num_as_str(i, buff=6)+".png");
show()
def test1():
print "getting empty map"
gm = get_empty_map()
print "getting probability map"
themap = gm.to_probability_map_carmen()
print "plotting map"
carmen_maptools.plot_map(themap, gm.x_size, gm.y_size)
print "saving map"
gm.save_carmen_map("empty_map.cmf.gz");
show()
def test1():
print "getting sawtooth map"
gm = get_spoke_map()
#print "getting probability map"
themap = gm.to_probability_map_carmen()
#print "plotting map"
carmen_maptools.plot_map(themap, gm.y_size, gm.x_size)
print "saving map"
gm.save_carmen_map("spoke_map.cmf.gz")
show()
def show_graph(gridmap, samples, labels, colors=None):
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size);
dists = get_graph(gridmap, samples)
for i in range(len(dists[0])):
for j in range(len(dists[1])):
if(dists[i,j] > 0):
plot([samples[0,i], samples[0,j]], [samples[1,i], samples[1,j]], 'k-')
show_partitions(gridmap, samples, labels, colors)
draw()
def test4():
ion()
#class noise_model2D{
tv_nm = noise_model2D(1.0, 0, 0.1, 0.0)
rv_nm = noise_model2D(0, 1.0, 0.0, 0.1)
slip_nm = noise_model2D(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
onm = observation_noise_model(0.1, 0.01, 0.0001)
x, y, theta = [6, 6, 0]
ax = gca()
#make a simulator, load and plot the map
#print "starting simulator"
mysim = simulator2D(nm, onm, "/home/tkollar/repositories/tklib/data/maps/hurdles.cmf.gz");
#print "occupancy grid mapper"
mymap = occupancy_grid_mapper(0.9,0.9, 0.5, 20, 20, 0.1,
mysim.get_pose(), 0.3, -pi/2.0, pi/2.0)
myid = carmen_maptools.plot_map(mymap.map.to_probability_map_carmen(),
mymap.map.x_size, mymap.map.y_size);
nn_plt, = plot([], [], 'ro');
orig_plt, = plot([], [], 'bx');
icp_plt, = plot([], [], 'gx');
robot_pose_plt, = plot([], [], 'ko');
robot_orient_plt, = plot([], [], 'k');
#do some simulation
for i in range(1000):
mysim.simulate_motion(1.0, 0.6, 0.1);
sim_meas = mysim.simulate_measurements();
if(len(sim_meas) > 0):
mymap.update(mysim.get_pose(), sim_meas);
if(mod(i, 5) == 0):
print mymap.map.to_probability_map()
carmen_maptools.plot_map(mymap.map.to_probability_map_carmen(),
mymap.map.x_size, mymap.map.y_size);
ax.images = [ax.images[len(ax.images)-1]]
x, y, theta = mysim.get_pose();
robot_pose_plt.set_data([x], [y]);
robot_orient_plt.set_data([x, x+0.3*cos(theta)], [y, y+0.3*sin(theta)]);
draw()
show()
def show_partitions(gridmap, samples, labels, colors_in=None, show_map=True):
#plot the samples
samples = array(samples)
if(colors_in == None):
plcolors = get_colors()
else:
plcolors = colors_in
pltypes = ['o','^','<','>','s','d','p','h','x']
numclasses = max(labels)+1
colorNum = 0
typeNum = 0
myplots = []
mylabels = []
for cl in range(numclasses):
#if(mod(cl, len(pltypes)-1) == 0):
# typeNum += 1
if(colorNum >= len(plcolors)):
colorNum = 0
if(typeNum >= len(pltypes)):
typeNum = 0
color = plcolors[colorNum]
ttype = pltypes[typeNum]
for i in range(len(labels)):
if(labels[i] == cl):
#print int(labels[i])
p1, = plot([samples[0,i]], [samples[1,i]], ttype, mfc=color)
#plcolors[int(labels[i])])
if(not str(cl) in mylabels):
myplots.append(p1)
mylabels.append(str(cl))
colorNum += 1
typeNum += 1
legend(myplots, mylabels)
if(show_map):
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size,
gridmap.x_offset, gridmap.y_offset);
draw()
def mypartition_map(argv):
if (len(argv) == 7):
#load the gridmap
mp = map_partitioner(argv[1],
float(argv[2]),
int(argv[3]),
int(argv[4]),
gridcell_skip=float(argv[5]))
outfilename = argv[6]
elif (len(argv) == 4):
mp = map_partitioner(argv[1], gridcell_skip=float(argv[2]))
outfilename = argv[3]
elif (len(argv) == 5):
mp = map_partitioner(argv[1],
float(argv[2]),
gridcell_skip=float(argv[3]))
outfilename = argv[4]
else:
print "usage:"
print "\t >>python partition_map.py skel_filename [alpha, numclasses numsamples] gridcell_skip=15.0 outfile"
return
print "running"
mp.run()
print "showing explosion"
show_explosion(mp.get_map(), mp.samples, mp.labels)
savefig(outfilename + ".explosion.eps")
#make a new figure
figure()
#show the map
print "showing partitions"
show_partitions(mp.get_map(), mp.samples, mp.labels)
savefig(outfilename + ".partitions.eps")
figure()
themap = mp.get_map().to_probability_map_carmen()
carmen_maptools.plot_map(themap,
mp.get_map().x_size,
mp.get_map().y_size)
#show the explosion
savefig(outfilename + ".map.eps")
show()
mp.gridmap = None
mp.skel.gridmap = None
dump(mp, open(outfilename + ".pck", 'w'))
def test1(filename, num_iterations):
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(filename)
#make a binary map from a gridmap
b_map = make_binary_map(gridmap)
b_map = median_filter(b_map, 6)
#imshow(filtered_map, origin=1, cmap=cm.gray)
#figure()
for i in range(num_iterations):
if (i % 2 == 0):
b_map, num_deleted = skeletonize(b_map, True)
else:
b_map, num_deleted = skeletonize(b_map, False)
imshow(b_map, origin=1, cmap=cm.gray)
if (num_deleted < 2):
break
#show the staircase removed map
figure()
curr_map = remove_staircases(b_map)
imshow(curr_map, origin=1, cmap=cm.gray)
title("staircases removed")
#show the control point map
figure()
junction_map, Ij = find_junction_points(curr_map)
imshow(junction_map, origin=1, cmap=cm.gray)
title("junction points")
figure()
end_map, Ie = find_end_points(curr_map)
imshow(end_map, origin=1, cmap=cm.gray)
title("end points")
figure()
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
XYj = ind_to_xy(gridmap, Ij)
XYe = ind_to_xy(gridmap, Ie)
pj, = plot(XYj[0], XYj[1], 'bo')
pe, = plot(XYe[0], XYe[1], 'g^')
legend((pj, pe), ("Junction Points", "End Points"), shadow=True)
show()
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 test2():
ion()
#class noise_model2D{
tv_nm = noise_model2D(1.0, 0, 0.1, 0.0)
rv_nm = noise_model2D(0, 1.0, 0.0, 0.1)
slip_nm = noise_model2D(0, 0, 0.01, 0.0)
#make the papa of noise models here
nm = motion_noise_model_slip(tv_nm, rv_nm, slip_nm)
x, y, theta = [6, 6, 0]
#plot the measurements
reading_plt, = plot([], [], 'ro');
#true plots
position_plt, = plot([], [], 'go');
orient_plt, = plot([], []);
#corrupted plots
positionc_plt, = plot([], [], 'ko');
orientc_plt, = plot([], []);
#make a simulator, load and plot the map
mysim = simulator2D(nm, 0.1, "../../data/maps/hurdles.map.gz");
themap = mysim.get_map()
carmen_maptools.plot_map(themap, mysim.map.x_size, mysim.map.y_size);
#do some simulation
for i in range(300):
mysim.simulate_motion(1.0, 0.6, 0.1);
sim_meas = mysim.simulate_measurements();
if(len(sim_meas) > 0):
XY = carmen_util_reading_to_xy(mysim.get_true_pose(), sim_meas, -pi/2.0, pi/2.0);
XYc = carmen_util_reading_to_xy(mysim.get_pose(), sim_meas, -pi/2.0, pi/2.0);
x, y, theta = mysim.get_true_pose()
xc, yc, thetac = mysim.get_pose()
#do some plotting
reading_plt.set_data(XY[0], XY[1]);
position_plt.set_data([x], [y]);
orient_plt.set_data([x, x+cos(theta)], [y, y+sin(theta)]);
positionc_plt.set_data([xc], [yc]);
orientc_plt.set_data([xc, xc+cos(thetac)], [yc, yc+sin(thetac)]);
draw()
def plot_map(self, mymap, i):
if (show_map):
x, y, theta = self.current_pose
self.robot_pose_plt.set_data([x], [y])
self.robot_orient_plt.set_data([x, x + 1.5 * cos(theta)],
[y, y + 1.5 * sin(theta)])
if (mod(i, 10) == 0):
curr_map = mymap.map.downsample_map(2)
carmen_maptools.plot_map(curr_map,
mymap.map.x_size,
mymap.map.y_size,
cmap="binary",
curraxis=self.ax)
self.ax.images = [self.ax.images[len(self.ax.images) - 1]]
draw()
def map_to_mat(filename, outfilename):
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(filename);
themap = gridmap.to_probability_map_carmen();
map_dict = {}
map_dict['map'] = themap
map_dict['x_size'] = gridmap.x_size
map_dict['y_size'] = gridmap.y_size
map_dict['resolution'] = gridmap.resolution
print "saving matlab file"
savemat(outfilename, map_dict)
print "ploting map"
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size);
show()
def test1(filename):
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(filename)
themap = gridmap.to_probability_map_carmen()
figure()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
#print "starting computation"
#print "number of boundary points", len(bd[0,:])
bd = compute_boundary_cells(gridmap)
X, Y = ind_to_xy(gridmap, bd)
plot(X, Y, 'ro')
figure()
vd = compute_distance_transform_skeleton(gridmap)
imshow(vd, origin=1, cmap=cm.gray)
show()
def test1():
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map("/home/tkollar/installs/carmen/data/thickwean.map")
#test get
#print "getting free location"
#gridmap.get_random_free_location(0.25)
print "adding hurdles"
gridmap.add_hurdles(0.2413, 0.03, 10)
themap = gridmap.to_probability_map_carmen()
print "saving map"
success = gridmap.save_carmen_map("./test2.map")
print "success", success
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
show()
def test1():
gridmap = tklib_log_gridmap()
print "loading thickwean"
gridmap.load_carmen_map("/home/tkollar/installs/carmen/data/thickwean.map")
loc = gridmap.get_random_open_location(0.2)
plot([loc[0]], [loc[1]], 'b^')
locations = []
pts = gridmap.get_free_locations()
X, Y = pts
plot(X, Y, 'rx')
themap = gridmap.to_probability_map_carmen()
#print themap
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
show()
def test1():
gridmap = tklib_gridmap()
gridmap.load_carmen_map(
"/home/tkollar/repositories/tklib/pytools/map_partitioning/maps/ubremen-cartesium/cartesium.cmf.gz"
)
themap = gridmap.get_map()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
figure()
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(
"/home/tkollar/repositories/tklib/pytools/map_partitioning/maps/ubremen-cartesium/cartesium.cmf.gz"
)
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
show()
def test1():
figure()
gridmap = tklib_log_gridmap()
gridmap.load_carmen_map(
"/home/tkollar/repositories/tklib/pytools/map_partitioning/maps/ubremen-cartesium/cartesium.cmf.gz"
)
x, y = gridmap.get_random_open_location(0.2)
thetas = arange(0, 2 * pi, pi / 180)
myrange = gridmap.ray_trace(x, y, thetas)
X = myrange * cos(thetas) + x
Y = myrange * sin(thetas) + y
plot([x], [y], 'ro')
plot(X, Y, 'gx')
themap = gridmap.to_probability_map_carmen()
carmen_maptools.plot_map(themap, gridmap.x_size, gridmap.y_size)
show()
