Анализ и сравнение алгоритмов маршрутизации для домашних роботов

title («Robot testing»)#Menum = Menu (root)root.config (menu=m)cm = Menu (m)m.add_cascade (label="Файл", menu=cm)cm.add_command (label="Выход", command=exit)startXY = 150#координаты x, y начала поляn = 20#поле NxMm = 20#mas1 = [[0] * m for i in range (n)]#массив для поляmas2 = [[0] * mforiinrange (n)]#массив для запоминания очищенных клетокsizeR = 20#размер клеток поля#функция задает параметры для отрисовки стенки периметра комнатыdef initWall ():global mglobal mas1for i in range (len (mas1)):for j in range (len (mas1[i])):mas1[i][j] = 0#полеmas2[i][j] = 0#подсчет столкновенийfor i in range (len (mas1)):mas1[i][0] = 1mas1[0][i] = 1mas1[i][m-1] = 1mas1[m-1][i] = 1hasMap = False #имеется ли отрисованная карта#функция задает параметры для отрисовки карты № 1def loadMap1():global mas1global hasMaphasMap = Truemas1[5][7] = 1mas1[6][7] = 1mas1[5][8] = 1mas1[6][8] = 1mas1[5][10] = 1mas1[6][10] = 1mas1[5][11] = 1mas1[6][11] = 1mas1[11][7] = 1mas1[12][7] = 1mas1[11][8] = 1mas1[12][8] = 1mas1[11][10] = 1mas1[12][10] = 1mas1[11][11] = 1mas1[12][11] = 1for y in range (6, 13):for x in range (7, 11):mas1[x][y] = 1#функция задает параметры для отрисовки карты № 2def loadMap2():global mglobal mas1global hasMaphasMap = True#for i in range (len (mas1)):#mas1[m-2][i] = 1for y in range (5, 12):for x in range (17, 19):mas1[x][y] = 1for y in range (12, 19):for x in range (1, 3):mas1[x][y] = 1mas1[3][17] = 1mas1[4][17] = 1mas1[3][18] = 1mas1[4][18] = 1#функция задает параметры для отрисовки карты № 3def loadMap3():global mglobal mas1global hasMaphasMap = Truemas1[5][6] = 1mas1[6][6] = 1mas1[5][7] = 1mas1[6][7] = 1mas1[5][9] = 1mas1[6][9] = 1mas1[5][10] = 1mas1[6][10] = 1mas1[11][6] = 1mas1[12][6] = 1mas1[11][7] = 1mas1[12][7] = 1mas1[11][9] = 1mas1[12][9] = 1mas1[11][10] = 1mas1[12][10] = 1for y in range (5, 12):for x in range (7, 11):mas1[x][y] = 1for y in range (5, 12):for x in range (17, 19):mas1[x][y] = 1for y in range (12, 19):for x in range (1, 3):mas1[x][y] = 1mas1[3][17] = 1mas1[4][17] = 1mas1[3][18] = 1mas1[4][18] = 1#emptyMap#функция задает параметры для отрисовки карты № 4def loadMap4():global hasMaphasMap = True#функция задает параметры для отрисовки карты № 5def loadMap5():global mas1global hasMaphasMap = Truemas1[5][7] = 1mas1[6][7] = 1mas1[5][8] = 1mas1[6][8] = 1mas1[5][10] = 1mas1[6][10] = 1mas1[5][11] = 1mas1[6][11] = 1mas1[11][7] = 1mas1[12][7] = 1mas1[11][8] = 1mas1[12][8] = 1for y in range (6, 13):for x in range (7, 11):mas1[x][y] = 1#функция задает параметры для отрисовки карты № 6def loadMap6():global mglobal mas1global hasMaphasMap = True#for i in range (len (mas1)):#mas1[m-2][i] = 1for y in range (5, 12):for x in range (17, 19):mas1[x][y] = 1for y in range (12, 19):for x in range (1, 3):mas1[x][y] = 1mas1[3][17] = 1mas1[4][17] = 1mas1[3][18] = 1mas1[4][18] = 1for y in range (10, 17):for x in range (11, 15):mas1[x][y] = 1#функция задает параметры для отрисовки карты № 7def loadMap7():global mglobal mas1global hasMaphasMap = Truemas1[5][6] = 1mas1[6][6] = 1mas1[5][7] = 1mas1[6][7] = 1mas1[5][9] = 1mas1[6][9] = 1mas1[5][10] = 1mas1[6][10] = 1mas1[11][6] = 1mas1[12][6] = 1mas1[11][7] = 1mas1[12][7] = 1mas1[11][9] = 1mas1[12][9] = 1mas1[11][10] = 1mas1[12][10] = 1for y in range (5, 12):for x in range (17, 19):mas1[x][y] = 1for y in range (12, 19):for x in range (1, 3):mas1[x][y] = 1mas1[3][17] = 1mas1[4][17] = 1mas1[3][18] = 1mas1[4][18] = 1#функция задает параметры для отрисовки карты № 8def loadMap8():global mglobal mas1global hasMaphasMap = Truemas1[5][6] = 1mas1[6][6] = 1mas1[5][7] = 1mas1[6][7] = 1mas1[5][9] = 1mas1[6][9] = 1mas1[5][10] = 1mas1[6][10] = 1mas1[11][6] = 1mas1[12][6] = 1mas1[11][7] = 1mas1[12][7] = 1mas1[11][9] = 1mas1[12][9] = 1mas1[11][10] = 1mas1[12][10] = 1for y in range (5, 12):for x in range (7, 11):mas1[x][y] = 1for y in range (5, 12):for x in range (17, 19):mas1[x][y] = 1for y in range (12, 19):for x in range (1, 3):mas1[x][y] = 1for y in range (3, 10):for x in range (1, 3):mas1[x][y] = 1#Обновление канвасаdef updateCanvace ():canv.delete («all»)canv.create_rectangle (0,0,w, h, outline='blue', width=10)timeExp = 0 #время эксперимента#загрузка картыdefloadMap (numMap):global timeRobotif numMap == 1: loadMap1()if numMap == 2: loadMap2()if numMap == 3: loadMap3()if numMap == 4: loadMap4()if numMap == 5: loadMap5()if numMap == 6: loadMap6()if numMap == 7: loadMap7()if numMap == 8: loadMap8()#tkMessageBox.showinfo («Say», timeExp) numberMap = 1#номер карты#Функция обрабатывает событие клика мышкой по кнопке Карта1def loadM1(event):global alreadyRunglobal numberMapnumberMap = 1if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта2def loadM2(event):global alreadyRunglobal numberMapnumberMap = 2if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта3def loadM3(event):global alreadyRunglobal numberMapnumberMap = 3if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта4def loadM4(event):global alreadyRunglobal numberMapnumberMap = 4if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта5def loadM5(event):global alreadyRunglobal numberMapnumberMap = 5if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта6def loadM6(event):global alreadyRunglobal numberMapnumberMap = 6if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта7def loadM7(event):global alreadyRunglobal numberMapnumberMap = 7if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция обрабатывает событие клика мышкой по кнопке Карта8def loadM8(event):global alreadyRunglobal numberMapnumberMap = 8if alreadyRun == False: updateCanvace ()initWall ()loadMap (numberMap)createField ()#Функция рисует полеdef createField ():global mas1global startXYglobal sizeRx = startXYy = startXYfor i in range (len (mas1)):for j in range (len (mas1[i])):if mas1[i][j] == 1: canv. create_rectangle (x, y, x+sizeR, y+sizeR, fill="black", outline="blue")else:canv.create_rectangle (x, y, x+sizeR, y+sizeR, fill="gray", outline="blue")y = y + sizeRx = x + sizeRy = startXYii = 0jj = 0#Функция задает начальные значения роботуdef initRobot (startRobotX=2, startRobotY=4):global iiglobal jjglobal startXYglobal sizeRii=startRobotXjj=startRobotYclearTest ()canv.create_oval (startXY+startRobotX*sizeR, startXY+startRobotY*sizeR,(startXY+startRobotX*sizeR)+sizeR,(startXY+startRobotY*sizeR)+sizeR, fill="green", outline='green')Collision = 0 #столкновенияalreadyRun = False #проверка не запущен ли уже алгоритмfuncFront = 1 #направление движенияstopRobot = True #остановить роботаscoreRobot = 100 #скоростьtest1₃ = False #проверка было ли столкновения по пути 1_3test3₁ = False #проверка было ли столкновения по пути 3_1test4₂ = False #проверка было ли столкновения по пути 4_2test2₄ = False #проверка было ли столкновения по пути 2₄#=================ZigZag=====================def start1():global test3_1global test1_3global test4_2global test2_4global mas1global stopRobotglobal iiglobal jjglobal funcFrontglobal Collisionglobal timeRobotglobal timeExpif timeRobot >= timeExp: stop_Robot ()else:timeRobot = timeRobot + 1if stopRobot==False:if (ii<m-1) and (jj<n-1):if funcFront == 1: if test1₃ == True: if mas1[ii][jj+1]==0:Step (2)funcFront = 3clearTest ()elif mas1[ii-1][jj]==0:Collision = Collision + 1Step (3)elif mas1[ii+1][jj]==0 and test1₃ == False: Step (1)elif mas1[ii+1][jj]==1 and test1₃ == False: Collision = Collision +1test1₃ = Trueelif funcFront == 3: if test1₃ == True: if mas1[ii][jj+1]==0:Step (2)funcFront = 1clearTest ()elif mas1[ii+1][jj]==0:Collision = Collision + 1Step (1)elif mas1[ii-1][jj]==0 and test1₃ == False: Step (3)elif mas1[ii-1][jj]==1 and test1₃ == False: Collision = Collision +1test1₃ = Trueroot. after (scoreRobot, start1)#=================Random=====================ii2 = 0 #координата x направления роботаjj2 = 1 #координата y направления роботаcountLenthRandomStart = 7 #количество клеток для движения без перерываcountLenthRandom = countLenthRandomStartdef start2():global countLenthRandomglobal countLenthRandomStartglobal mas1global stopRobotglobal iiglobal jjglobal ii2global jj2global funcFrontglobal Collisionglobal timeRobotglobal timeExpif timeRobot >= timeExp: stop_Robot ()else:timeRobot = timeRobot + 1if stopRobot==False:countLenthRandom = countLenthRandom — 1if countLenthRandom == 0: getRandom ()countLenthRandom = countLenthRandomStartwhile ii2==0 and jj2 == 0: getRandom ()if mas1[ii+ii2][jj+jj2]==0:Step (10)else:Collision = Collision +1getRandom ()countLenthRandom = countLenthRandomStartroot. after (scoreRobot, start2)#=================Спираль=====================countLenthSpStart = 1 #начальный шагcountLenthSp = countLenthSpStartdef start3():global countLenthSpStartglobal countLenthSpglobal mas1global stopRobotglobal iiglobal jjglobal funcFrontglobal Collisionglobal timeRobotglobal timeExpif timeRobot >= timeExp: stop_Robot ()else:timeRobot = timeRobot + 1if stopRobot==False:#print funcFrontif funcFront == 1:#print funcFrontif countLenthSp > 0: countLenthSp = countLenthSp — 1if mas1[ii+1][jj]==0:Step (1)elif mas1[ii][jj-1]==0:Collision = Collision +1Step (4)else:funcFront = 2countLenthSp = countLenthSpStartelif funcFront == 2: if countLenthSp > 0: countLenthSp = countLenthSp — 1if mas1[ii][jj+1]==0:Step (2)elif mas1[ii+1][jj]==0:Collision = Collision +1Step (1)else:funcFront = 3countLenthSpStart = countLenthSpStart + 1countLenthSp = countLenthSpStartelif funcFront == 3: if countLenthSp > 0: countLenthSp = countLenthSp — 1if mas1[ii-1][jj]==0:Step (3)elif mas1[ii][jj+1]==0:Collision = Collision +1Step (2)else:funcFront = 4countLenthSp = countLenthSpStartelif funcFront == 4: if countLenthSp > 0: countLenthSp = countLenthSp — 1if mas1[ii][jj-1]==0:Step (4)elif mas1[ii-1][jj]==0:Collision = Collision +1Step (3)else:funcFront = 1countLenthSpStart = countLenthSpStart + 1countLenthSp = countLenthSpStartroot. after (scoreRobot, start3)#очистка сообщений о столкновенииdefclearTest ():global test3_1global test1_3global test4_2global test2_4test1₃ = Falsetest3₁ = Falsetest4₂ = Falsetest2₄ = False#шаг в направлении stdef Step (st):global sizeRglobal iiglobal jjglobal ii2global jj2global mas2#if (ii+1<range (len (mas1)) and (jj+1<range (len (mas1[ii]))):mas2[ii][jj]=1if st==1:x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintR (x, y, x2,y2)ii = ii+1x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintO (x, y, x2,y2)elif st==2:x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintR (x, y, x2,y2)jj = jj+1x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintO (x, y, x2,y2)elif st==3:x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintR (x, y, x2,y2)ii = ii-1x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintO (x, y, x2,y2)elif st==4:x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintR (x, y, x2,y2)jj = jj-1x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintO (x, y, x2,y2)elif st==10:x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintR (x, y, x2,y2)jj = jj + jj2ii = ii + ii2x = startXY+ii*sizeRy = startXY+jj*sizeRx2 = (startXY+ii*sizeR)+sizeRy2 = (startXY+jj*sizeR)+sizeRpaintO (x, y, x2,y2)#нарисовать клеткуdefpaintR (x, y, x2,y2):canv.create_rectangle (x, y, x2,y2,fill="yellow", outline="blue")#нарисовать кружок (робота)defpaintO (x, y, x2,y2):canv.create_oval (x, y, x2,y2,fill="green", outline='green')#запуск роботаdef initRunRobot (x, y):global alreadyRunglobal stopRobotglobal funcFrontglobal numberMapglobal lbl1global timeExpif alreadyRun == False and hasMap == True: timeExp = int (text1.get ('1.0', END))alreadyRun = TruestopRobot = FalseinitWall ()loadMap (numberMap)createField ()initRobot (x, y) funcFront = 1#Функция обрабатывает событие клика мышкой по кнопке Старт. Зигзагdef runRobot1(event):global alreadyRunglobal hasMapif alreadyRun == False and hasMap == True: initRunRobot (1,1)start1()#Функция обрабатывает событие клика мышкой по кнопке Старт. Спиральdef runRobot3(event):global numberMapif numberMap == 1 or numberMap == 3 or numberMap == 5 or numberMap == 8: runRobot3₁()else:runRobot3₂()#При выборе 1-й и 3-й карты при алгоритме Спиральdef runRobot3₁():global alreadyRunglobal hasMapglobal countLenthSpStartglobal countLenthSpcountLenthSpStart = 1countLenthSp = countLenthSpStartif alreadyRun == False and hasMap == True: initRunRobot (3,3)start3()#При выборе 2-й и 4-й карты при алгоритме Спиральdef runRobot3₂():global alreadyRunglobal hasMapglobal countLenthSpStartglobal countLenthSpcountLenthSpStart = 1countLenthSp = countLenthSpStartif alreadyRun == False and hasMap == True: initRunRobot (9,9)start3()#Функция задает координаты вектора направления роботаdef getRandom ():global ii2global jj2ii2 = int (random.uniform (-2, 2))jj2 = int (random.uniform (-2, 2))#Функция обрабатывает событие клика мышкой по кнопке Старт. Рандомdef runRobot2(event):global ii2global jj2global alreadyRunglobal hasMapif alreadyRun == False and hasMap == True: initRunRobot (1,1)getRandom ()start2()#Лейблы для сообщенийlbl1 = Label (root, text="", width=30)lbl2 = Label (root, text="", width=30)lbl3 = Label (root, text="", width=30)lbl4 = Label (root, text="", width=30)countCellClear=0 #количество чистых клетокtimeRobot = 0 #затраченное время#Функция обрабатывает событие клика мышкой по кнопке Стопdef stopR (event):stop_Robot ()#Функция останавливает роботаdefstop_Robot ():global stopRobotglobal alreadyRunglobal Collisionglobal mas1global mas2global lbl1global lbl2global lbl3global lbl4global countCellClearglobal timeRobotglobal timeExpif alreadyRun == True: alreadyRun = FalsestopRobot = TruetimeExp = 0countFree = 0for i in range (len (mas1)):for j in range (len (mas1[i])):if mas2[i][j] == 1: countCellClear=countCellClear+1if mas1[i][j] == 0: countFree=countFree+1lbl1.destroy ()lbl2.destroy ()lbl3.destroy ()lbl4.destroy ()lbl1.text = «Количество столкновений = %s» %(Collision)lbl1 = Label (root, text="Количество столкновений = %s" %(Collision), width=30)lbl1.place (relx=.84, rely=relyButton+0.2, anchor="c")if (countCellClear+1==325):countCellClear=323lbl2 = Label (root, text="Количество очищенных клеток = %s" %(countCellClear+1), width=30)lbl2.place (relx=.84, rely=relyButton+0.3, anchor="c")lbl3 = Label (root, text="Затраченное время = %s" %(timeRobot), width=30)lbl3.place (relx=.84, rely=relyButton+0.4, anchor="c")percent=((countCellClear+1)*100)/ float (countFree)percent=round (percent, 2) lbl4 = Label (root, text="Очищенная площадь = %s%s" %(percent," %"), width=30)lbl4.place (relx=.84, rely=relyButton+0.5, anchor="c")Collision = 0countCellClear=0timeRobot = 0#увеличение скоростиdef sel1(): global scoreRobot scoreRobot = 100#уменьшение скоростиdef sel2(): global scoreRobot scoreRobot = 20w=900#ширина окнаh=600#высота окна#канвасcanv=Canvas (root, width=w, height=h, bg='#FFFFFF')canv.create_rectangle (0,0,w, h, outline='blue', width=10)#настройка окна окнаroot. minsize (width = w, height=h)root.maxsize (width = w, height=h)#вспомогательные координаты для настройки местоположения кнопок и лейбловrelyButton = 0.08relyButtonStart = 0.22#=====настройка местоположения кнопок и лейблов========bs1 = Button (root, text="Старт. Зигзаг.")bs1.bind («<Button-1>», runRobot1) bs1. place (relx=.24, rely=relyButtonStart, anchor="c")bs2 = Button (root, text="Старт. Рандом.")bs2.bind («<Button-1>», runRobot2) bs2. place (relx=.4, rely=relyButtonStart, anchor="c")bs3 = Button (root, text="Старт. Спираль.")bs3.bind («<Button-1>», runRobot3) bs3. place (relx=.57, rely=relyButtonStart, anchor="c")bstop = Button (root, text="Стоп")bstop .bind («<Button-1>», stopR) bstop. place (relx=.2, rely=relyButton, anchor="c")bM1 = Button (root, text="Карта1″)bM1.bind («<Button-1>», loadM1) bM1. place (relx=.3, rely=relyButton, anchor="c")bM2 = Button (root, text="Карта2″)bM2.bind («<Button-1>», loadM2) bM2. place (relx=.4, rely=relyButton, anchor="c")bM3 = Button (root, text="Карта3″)bM3.bind («<Button-1>», loadM3) bM3. place (relx=.5, rely=relyButton, anchor="c")bM4 = Button (root, text="Карта4″)bM4.bind («<Button-1>», loadM4) bM4. place (relx=.6, rely=relyButton, anchor="c")relyButton = relyButton + .07bM5 = Button (root, text="Карта5″)bM5.bind («<Button-1>», loadM5) bM5. place (relx=.3, rely=relyButton, anchor="c")bM6 = Button (root, text="Карта6″)bM6.bind («<Button-1>», loadM6) bM6. place (relx=.4, rely=relyButton, anchor="c")bM7 = Button (root, text="Карта7″)bM7.bind («<Button-1>», loadM7) bM7. place (relx=.5, rely=relyButton, anchor="c")bM8 = Button (root, text="Карта8″)bM8.bind («<Button-1>», loadM8) bM8. place (relx=.6, rely=relyButton, anchor="c")lblTime = Label (root, text="Время = «, width=10)lblTime.place (relx=.80, rely=relyButton-0.05, anchor="c»)text1=Text (root, height=1,width=7,font='Arial 14', wrap=WORD)text1.place (relx=.90, rely=relyButton-0.05, anchor="c")lbl1 = Label (root, text="Количество столкновений = «, width=30)lbl1.place (relx=.84, rely=relyButton+0.2, anchor="c»)lbl2 = Label (root, text="Количество очищенных клеток = «, width=30)lbl2.place (relx=.84, rely=relyButton+0.3, anchor="c»)lbl3 = Label (root, text="Затраченное время = «, width=30)lbl3.place (relx=.84, rely=relyButton+0.4, anchor="c»)lbl4 = Label (root, text="Очищенная площадь = «, width=30)lbl4.place (relx=.84, rely=relyButton+0.5, anchor="c»)var = IntVar ()R1 = Radiobutton (root, text="Нормальная скорость", variable=var, value=1, command=sel1)R1.select ()R1.place (relx=.84, rely=relyButton, anchor="c")R2 = Radiobutton (root, text="Высокая скорость «, variable=var, value=2, command=sel2)R2.place (relx=.84, rely=relyButton+0.08, anchor="c»)#———настройка местоположения кнопок и лейблов————canv.pack ()root.mainloop ().