一个简单的全覆盖路径规划python

#!/usr/bin/python
# -----------
#


# ----------


import numpy as np
import math
import matplotlib.pyplot as plt

import sys pygame
from pygame.locals import *

rows = 10
columns = 10

rows_space = 40
columns_space = 40

# Grid specs
WIDTH = 5
ROWS = 20
COLUMNS = 20
SQUARE_SIDE = 30
EXT_GRID = SQUARE_SIDE * ROWS
GRID_THICK = 1

SCREEN_SIDE = SQUARE_SIDE * ROWS

SCREEN_OFFSET = 20
SCREEN_SIZE = （ROWS * （SQUARE_SIDE） + 2 * SCREEN_OFFSET COLUMNS * （SQUARE_SIDE） + 2 * SCREEN_OFFSET）

# COLORS
green = 0 255 0
red = 255 0 0
blue = 0 0 255
white = 255 255 255

# 2 obstacles
# 0 free
# 1 free cleaned
grid = np.zeros（[ROWS COLUMNS]）

# OBSTACLES

# Fig. 5
‘‘‘
grid[4][5] = 2
grid[4][6] = 2
grid[4][7] = 2
grid[4][8] = 2
grid[4][9] = 2
grid[4][10] = 2
grid[4][11] = 2
grid[4][12] = 2
grid[4][13] = 2
grid[4][14] = 2
grid[4][15] = 2
grid[4][16] = 2
grid[4][17] = 2
grid[4][18] = 2
grid[4][19] = 2
‘‘‘
grid[4][5] = 2
grid[4][6] = 2

grid_shape = grid.shape

print
“ grid shape “ + str（grid.shape）

# print len（grid） # n = 5
# print len（grid[0]） # m = 6
# print grid[1][0]

# TODO: initial point heading
#

initial = [0 0]
init = [0 0]
goal = [len（grid） - 1 len（grid[0]） - 1]
cost = 1

delta = [[-1 0]  # go up
         [0 -1]  # go left
         [1 0]  # go down
         [0 1]]  # go right

backtracking_list = []


class Node:

    def __init__（self column row cost pind）:
        self.column = column
        self.row = row
        self.cost = cost
        self.pind = pind  # parent indice

    def __str__（self）:
        return str（self.x） + ““ + str（self.y） + ““ + str（self.cost） + ““ + str（self.pind）


# def path（grid initial_point）:

def verify（direction row column）:
    if direction == ‘north‘:
        if row <= 0:
            return False
        elif grid[row - 1][column] == 1 or grid[row - 1][column] == 2:
            return False
        else:
            return True

    elif direction == ‘south‘:

        if row >= （grid.shape[0] - 1）:
            return False
        elif grid[row + 1][column] == 1 or grid[row + 1][column] == 2:
            return False
        else:
            return True

    elif direction == ‘west‘:

        if column <= 0:
            return False
        elif grid[row][column - 1] == 1 or grid[row][column - 1] == 2:
            return False
        else:
            return True

    elif direction == ‘east‘:

        if column >= （grid.shape[1] - 1）:
            return False
        elif grid[row][column + 1] == 1 or grid[row][column + 1] == 2:
            return False
        else:
            return True


def valid_cell（grid row column）:
    “““ Verify is the cell is valid. “““
    if （row < grid.shape[0] and row >= 0） and （column < grid.shape[1] and column >= 0）:
        return True
    else:
        return False


def backtracking_points（grid row