5. Алгоритм навигации робота

cd( @__DIR__ )
include( "room_functions.jl" );
gr()

using Random
Random.seed!(3);

function поворот( a::CartesianIndex, b::Number )
    vecA = [ a[1], a[2] ];
    rotM = [  cosd(b) sind(b)
             -sind(b) cosd(b) ]
    CartesianIndex( (Int.(round.(rotM * vecA )))...)
end;

function get_path( координаты_старта, направление, изученная_карта, пройденная_карта )
    
    #################
    # Заполняем маску
    #################
    
    wavefront = zeros( size(изученная_карта) )         # поле отмечаем цифрой 0 - по этим точкам можно ходить
    wavefront[ изученная_карта .== здесь_стена] .= -1  # точки известных нам стен отметим -1 (скорее для визуализации)
    wavefront[ координаты_старта ] = 2                 # отправную точку робота отметим цифрой 2
    
    wavefront[ (пройденная_карта .== не_посещали) .& (изученная_карта .== не_знаю) ] .= 1   # все точки, куда мы хотим попасть, отмечаем цифрой 1
    
    vec_coords = CartesianIndices( size(wavefront) )
    
    ##############################################
    # Размечаем маску и находим потенциальную цель
    ##############################################
    
    маршрут = []
    for cntr in 1:200
        if length(маршрут) > 0 break; end;
        
        периметр = vec_coords[ wavefront .== maximum(wavefront) ]
        for точка in периметр
            if length(маршрут) > 0 break; end;
            
            набор_направлений = [ направление, поворот(направление, 90), поворот(направление, -90), поворот(направление, 180) ]
            for направление in набор_направлений
                
                if wavefront[ точка + направление ] == 1
                    # Волшебно, мы нашли интересующую нас точку, можно заканчивать поиск
                    push!( маршрут, точка + направление )
                    push!( маршрут, точка );
                    break;
                end;
                
                if wavefront[ точка + направление ] == 0  # Шагаем в ту точку, до которой еще не расчитали маршрут
                    if изученная_карта[ точка + направление ] != здесь_стена wavefront[ точка + направление ] = wavefront[ точка ] + 1; end
                end
            end
            
        end
    end
    
    #############################################
    # Реконструкция маршрута по заполненной маске
    #############################################
    
    if length(маршрут) > 0
        for p in 1:500
            
            # Маршрут заполняется в обратную сторону
            # - первая точка маршрута – это неоткрытая точка карты
            # - последняя точка – текущее положение робота
            последняя_точка = маршрут[end];
            if wavefront[последняя_точка] == 2 break; end;
            
            # Отфильтруем соседние точки маски, которые могут быть продолжением маршрута
            соседние_точки = repeat( [последняя_точка], inner=4) .+ CartesianIndex.([(1,0), (0,1), (-1,0), (0,-1) ])
            соседние_точки = [ x for x in соседние_точки if x in vec_coords ] # Проверим что мы в пределах поля
            соседние_точки = [ x for x in соседние_точки if wavefront[x] >= 2 ] # Проверим что это не стены и не недоступное поле
            
            # Добавим минимальную точку в маршрут
            (_,i) = findmin( wavefront[ соседние_точки ] )
            push!( маршрут, соседние_точки[i] );
            
        end
    end
    
    return маршрут
    
end;

##################################
# Наша логика обозначений на карте
##################################

стена = true; проезд = false;
не_посещали = 1; посетили = 2;
не_знаю = 1; здесь_стена = 2; можно_ехать = 3;

#####################################
# Функция перемещения робота по карте
#####################################

function шагаем( положение, направление, маршрут, изученная_карта, пройденная_карта, вся_карта )
    
    конец_процедуры = false                        # Пока мы едем
    пройденная_карта[ положение ] = посетили       # Мы только приехали в эту точку
    
    сенсоры = [положение + направление, положение + направление + поворот( направление, 90 ), положение + направление + поворот( направление, -90 )]
    изученная_карта[ сенсоры ] = map( сенсор -> (вся_карта[сенсор] == стена ? здесь_стена : можно_ехать ), сенсоры )
    
    # Если маршрут пуст, вычислим его
    if length(маршрут) == 0
        маршрут = get_path( положение, направление, изученная_карта, пройденная_карта )
        # Если процедура ничего не вернула, значит достижимых неоткрытых точек больше нет
        if length(маршрут) == 0 конец_процедуры = true;
        else маршрут = маршрут[1:end-1]; end # А если маршрут есть, то он минимум в 2 точки длиной и первая -- координата робота
    end;
    
    if конец_процедуры == false
        
        следующая_точка = маршрут[end];
        
        точка_впереди = положение + направление
        if точка_впереди != следующая_точка
            # Проверим одно направление поворота. Если следующая точка там, то повернемся туда. Иначе повернемся в другом направлении
            if положение + поворот( направление, 90 ) == следующая_точка    направление = поворот( направление, 90 );
            else направление = поворот( направление, -90 ); end;
        else
            if вся_карта[ точка_впереди ] == проезд
                положение = точка_впереди;
                if length(маршрут) > 0 pop!( маршрут ); end; # Если мы успешно двинулись вперед, удалим последнюю точку из маршрута
            else
                # Если маршрут нас привел в стену и мы "изучили" эту точку, то устраним ее из маршрута
                if length(маршрут) > 0 pop!( маршрут ); end;
            end
        end
    end
    
    return положение, направление, маршрут, изученная_карта, пройденная_карта, конец_процедуры
end;

##################################
# Инициализация переменных
##################################

карта, старт_x, старт_y = gen_room_rend( 25, 20 )
старт = CartesianIndex( старт_x, старт_y )
положение = CartesianIndex( старт )
направление = CartesianIndex(1,0)

маршрут = []
путь = [ Tuple(старт) ];
изученная_карта = не_знаю .* ones( size(карта) ); # Теперь используем эту матрицу по назначению
изученная_карта[ старт ] = можно_ехать; # Робот определенно находится в точке, где нет стены
пройденная_карта = не_посещали .* ones( size(карта) );


a = Plots.Animation()
plt = plot();
i = 0

for i = 1:10000
    global plt
    положение, направление, маршрут, изученная_карта, пройденная_карта, конец_процедуры = шагаем( положение, направление, маршрут, изученная_карта, пройденная_карта, карта )
    
    if конец_процедуры == false push!(путь, Tuple(положение)); else break; end;
    
    plt = heatmap( ones(size(карта')), yflip=:true, aspect_ratio=:equal, leg=:false )
    spy!( plt, replace(изученная_карта' .== не_знаю, 0=>NaN), markersize=3, markercolor=:gray ) # c=:tab20, 
    heatmap!(plt, replace(изученная_карта', 1=>NaN) .- 2, yflip=:true, c=:greys, aspect_ratio=:equal, leg=:false );
    plot!( plt, first.(путь), last.(путь), aspect_ratio=:equal, lw=2, linez=range(0.0, stop=1.0, length=length(путь)), c=:batlow )
    if length(маршрут) > 0 scatter!( plt, [маршрут[1][1]], [маршрут[1][2]], markersize=5, markerstrokewidth=5, markercolor=:red, shape=:xcross ); end;
    
    # Отразим направление робота на графике треугольником
    if направление == CartesianIndex(1,0) scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:rtriangle, c=:red );
    elseif направление == CartesianIndex(0,1) scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:dtriangle, c=:red );
    elseif направление == CartesianIndex(-1,0) scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:ltriangle, c=:red );
    else scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:utriangle, c=:red ); end;
    
    frame(a, plt)
end


# И повторим последний кадр 50 раз
for i = 1:50
    frame(a, plt)
end


gif( a, "robot_animation.gif", fps=10 )

plot( heatmap( изученная_карта, title="изученная_карта"), heatmap( пройденная_карта, title="пройденная_карта" ), size=(1100,400))