X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Foa%2Fgraph.py;fp=projects%2Fmicrob2010%2Ftests%2Foa%2Fgraph.py;h=0b304bc2cc9cc402dfa1885d6350cd0c5c3729f1;hp=0000000000000000000000000000000000000000;hb=49fd019c8bd61d623accbd9eca62d88470370a3f;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py new file mode 100644 index 0000000..0b304bc --- /dev/null +++ b/projects/microb2010/tests/oa/graph.py @@ -0,0 +1,260 @@ +import sys, re, math +import numpy as np +import matplotlib +import matplotlib.path as mpath +import matplotlib.patches as mpatches +import matplotlib.pyplot as plt +from matplotlib.patches import Arrow, Circle, Wedge, Polygon +from matplotlib.collections import PatchCollection +import popen2, random + +OFFSET_CORN_X=150 +OFFSET_CORN_Y=222 +STEP_CORN_X=225 +STEP_CORN_Y=250 + +WAYPOINTS_NBX = 13 +WAYPOINTS_NBY = 8 + +TYPE_WAYPOINT=0 +TYPE_DANGEROUS=1 +TYPE_WHITE_CORN=2 +TYPE_BLACK_CORN=3 +TYPE_OBSTACLE=4 +TYPE_NEIGH=5 + +col = [TYPE_WAYPOINT] * WAYPOINTS_NBY +waypoints = [col[:] for i in range(WAYPOINTS_NBX)] +corn_table = [TYPE_WHITE_CORN]*18 + +corn_side_confs = [ + [ 1, 4 ], + [ 0, 4 ], + [ 2, 4 ], + [ 2, 3 ], + [ 0, 3 ], + [ 1, 3 ], + [ 1, 6 ], + [ 0, 6 ], + [ 2, 6 ], + ] +corn_center_confs = [ + [ 5, 8 ], + [ 7, 8 ], + [ 5, 9 ], + [ 7, 8 ], + ] + +Path = mpath.Path + +def mark_one_neigh(i, j): + global waypoints + if i < 0 or j < 0 or i >= WAYPOINTS_NBX or j >= WAYPOINTS_NBY: + return + if waypoints[i][j] != TYPE_WAYPOINT: + return + waypoints[i][j] = TYPE_NEIGH + +def mark_all_neigh(i, j): + global waypoints + if waypoints[i][j] != TYPE_WAYPOINT: + return + waypoints[i][j] = TYPE_NEIGH + mark_one_neigh(i,j+1) + mark_one_neigh(i,j-1) + mark_one_neigh(i+1,j) + mark_one_neigh(i-1,j) + if i & 1 == 0: + mark_one_neigh(i+1,j-1) + mark_one_neigh(i-1,j-1) + else: + mark_one_neigh(i+1,j+1) + mark_one_neigh(i-1,j+1) + +def pt2corn(i,j): + if i == 0 and j == 2: return 0 + if i == 0 and j == 4: return 1 + if i == 0 and j == 6: return 2 + if i == 2 and j == 3: return 3 + if i == 2 and j == 5: return 4 + if i == 2 and j == 7: return 5 + if i == 4 and j == 4: return 6 + if i == 4 and j == 6: return 7 + if i == 6 and j == 5: return 8 + if i == 6 and j == 7: return 9 + if i == 8 and j == 4: return 10 + if i == 8 and j == 6: return 11 + if i == 10 and j == 3: return 12 + if i == 10 and j == 5: return 13 + if i == 10 and j == 7: return 14 + if i == 12 and j == 2: return 15 + if i == 12 and j == 4: return 16 + if i == 12 and j == 6: return 17 + return -1 + +def corn_get_sym(i): + sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2] + return sym[i] + +def init_corn_table(conf_side, conf_center): + global corn_table, corn_side_confs, corn_center_confs + print "confs = %d, %d"%(conf_side, conf_center) + for i in range(18): + if i in corn_side_confs[conf_side]: + corn_table[i] = TYPE_BLACK_CORN + continue + if corn_get_sym(i) in corn_side_confs[conf_side]: + corn_table[i] = TYPE_BLACK_CORN + continue + if i in corn_center_confs[conf_center]: + corn_table[i] = TYPE_BLACK_CORN + continue + if corn_get_sym(i) in corn_center_confs[conf_center]: + corn_table[i] = TYPE_BLACK_CORN + continue + corn_table[i] = TYPE_WHITE_CORN + +def init_waypoints(): + global waypoints, corn_table + + for i in range(WAYPOINTS_NBX): + for j in range(WAYPOINTS_NBY): + # corn + c = pt2corn(i, j) + if c >= 0: + waypoints[i][j] = corn_table[c] + continue + # too close of border + if (i & 1) == 1 and j == WAYPOINTS_NBY -1: + waypoints[i][j] = TYPE_OBSTACLE + continue + # hill + if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2: + waypoints[i][j] = TYPE_OBSTACLE + continue + # dangerous points + if i == 0 or i == WAYPOINTS_NBX-1: + waypoints[i][j] = TYPE_DANGEROUS + continue + if (i&1) == 0 and j == WAYPOINTS_NBY-1: + waypoints[i][j] = TYPE_DANGEROUS + continue + + waypoints[i][j] = TYPE_WAYPOINT + + print i, waypoints[i] + return waypoints + +def build_poly(ptlist): + polydata = [] + polydata.append((Path.MOVETO, (ptlist[0]))) + for pt in ptlist[1:]: + polydata.append((Path.LINETO, (pt))) + polydata.append((Path.CLOSEPOLY, (ptlist[0]))) + codes, verts = zip(*polydata) + poly = mpath.Path(verts, codes) + x, y = zip(*poly.vertices) + return x,y + +def build_path(ptlist): + polydata = [] + polydata.append((Path.MOVETO, (ptlist[0]))) + for pt in ptlist[1:]: + polydata.append((Path.LINETO, (pt))) + codes, verts = zip(*polydata) + poly = mpath.Path(verts, codes) + x, y = zip(*poly.vertices) + return x,y + +def build_area(ax): + # area + x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)]) + ax.plot(x, y, 'g-') + + x,y = build_poly([(0,0), (0,500), (500,500), (500,0)]) + ax.plot(x, y, 'y-') + + x,y = build_poly([(3000,0), (3000,500), (2500,500), (2500,0)]) + ax.plot(x, y, 'b-') + + x,y = build_poly([(740,0), (740,500), (2260,500), (2260,0)]) + ax.plot(x, y, 'g--') + + # limit + #x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)]) + #ax.plot(x, y, 'g--') + + init_corn_table(random.randint(0,8), random.randint(0,3)) + + waypoints = init_waypoints() + mark_all_neigh(5,6) + + wcorn = [] + bcorn = [] + points = [] + dpoints = [] + neighs = [] + + i = 0 + j = 0 + x = OFFSET_CORN_X + while x < 3000: + if (i & 1) == 0: + y = OFFSET_CORN_Y + else: + y = OFFSET_CORN_Y + STEP_CORN_Y/2 + j = 0 + while y < 2100: + + if waypoints[i][j] == TYPE_WHITE_CORN: + wcorn.append(Circle((x, y), 25)) + if waypoints[i][j] == TYPE_BLACK_CORN: + bcorn.append(Circle((x, y), 25)) + elif waypoints[i][j] == TYPE_WAYPOINT: + points.append(Circle((x, y), 10)) + elif waypoints[i][j] == TYPE_DANGEROUS: + dpoints.append(Circle((x, y), 10)) + elif waypoints[i][j] == TYPE_NEIGH: + neighs.append(Circle((x, y), 10)) + + j += 1 + y += STEP_CORN_Y + i += 1 + x += STEP_CORN_X + + p = PatchCollection(wcorn, cmap=matplotlib.cm.jet, + alpha=0.5, facecolor=(.5,.5,1)) + ax.add_collection(p) + + p = PatchCollection(bcorn, cmap=matplotlib.cm.jet, + alpha=0.7, facecolor=(0,0,0)) + ax.add_collection(p) + + p = PatchCollection(points, cmap=matplotlib.cm.jet, + alpha=0.3, facecolor=(1,1,0)) + ax.add_collection(p) + + p = PatchCollection(dpoints, cmap=matplotlib.cm.jet, + alpha=0.5, facecolor=(1,0,1)) + ax.add_collection(p) + + p = PatchCollection(neighs, cmap=matplotlib.cm.jet, + alpha=1., facecolor=(1,0,0), + edgecolor=(1,0,0)) + ax.add_collection(p) + + +def graph(filename): + fig = plt.figure() + ax = fig.add_subplot(111) + + build_area(ax) + + ax.grid() + ax.set_xlim(-100, 3100) + ax.set_ylim(-100, 2200) + #ax.set_title('spline paths') + #plt.show() + fig.savefig(filename) + +graph("test.png")