General definitions

We define map logic and directions to use instead of numbers in further code.

# Map logic
const unknown = -1
const empty = 0
const wall = 1
const goal = 2
const goal_check = -2 # possible goal location

# Direction names
Cind = CartesianIndex
const forward = Cind(1,0)
const right = Cind(0,1)
const left = Cind(0,-1)
const backward = Cind(-1,0)
const dirs = (forward,right,left,backward)         # possible directions. Diagonal directions can also be added
const dir_inds = Dict((dirs[i], i) for i in 1:length(dirs))   # enumerating the directions

Dict{CartesianIndex{2}, Int64} with 4 entries:
  CartesianIndex(0, -1) => 3
  CartesianIndex(-1, 0) => 4
  CartesianIndex(1, 0)  => 1
  CartesianIndex(0, 1)  => 2

Robot definition

Robot is a structure. It remembers its own position and direction, studied and visited map. It has a set of possible moves it could make and a set of sensors: these are used to interact with the world.

When robot has found a goal to move to, it remembers the path (positions it needs to reach as well as moves it needs to make) and destination.

Finally, robot has some physical size, which can prevent it from moving too freely in the room.

mutable struct Robot
    pos::Union{CartesianIndex,Missing}
    dir::Union{CartesianIndex,Missing}

    map::Union{Matrix{Integer},Missing}
    visited::Union{BitMatrix,Missing}

    moves::Vector{Function}
    sensors::Vector{Function}

    path::Vector{Tuple{CartesianIndex,Integer}}
    destination::Union{CartesianIndex,Nothing}

    size::Rational  # Robot size to check if it can pass between walls, should be < 1
                    # Robot is a square for simplicity
                    # Should be rational to avoid inexact results when determining collisions
                    # due to inexact representation of floating point numbers
end;
Robot() = Robot(missing,missing,missing,missing,[],[],[],nothing,0);

Same robot (with same size, moves and sensors) can be used in different environments and set to do different goals. When set in a new environment, it needs some basic initialization, which we do here.

function init_memory!(robot::Robot,x,y,dx,dy,s_x,s_y)
    robot.pos = Cind(x,y)
    robot.dir = Cind(dx,dy)
    # Initialize outer walls, inner unknowns
    robot.map = ones(Integer,s_x,s_y)
    robot.map[2:s_x-1,2:s_y-1] .= unknown
    robot.map[x,y] = empty
    # These are unreachable anyway, so they will be empty
    robot.map[1,1] = empty
    robot.map[1,s_y] = empty
    robot.map[s_x,1] = empty
    robot.map[s_x,s_y] = empty
    # Nothing is visited
    robot.visited = falses(s_x, s_y)
    # except current position
    robot.visited[x,y] = true
    # Empty path and destination
    robot.path = []
    robot.destination = nothing
end;

Map definitions

Nothing serious was changed in terms of room functions from the course, so nothing to discuss here.

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

using Random
Random.seed!(3);

Auxiliary functions

Also the same as in the course.

function rotate( a::Cind, b::Number )
    vecA = [ a[1], a[2] ];
    rotM = [  cosd(b) sind(b)
             -sind(b) cosd(b) ]
    return Cind( (Int.(round.(rotM * vecA )))...)
end;

Interactions between robot and room

Robot interacts with the room either through movement or through sensors.

Movement

First, we have a general function, which checks for collisions in a straight path. Robot can use it too by calling it with room equal to its map.

function check_collisions(pos::Cind, dir::Cind, robot_size::Rational, room)
    dx = dir[1]//1
    dy = dir[2]//1
    return check_collisions(pos, dx, dy, robot_size, room)
end

# Checks the existance of a straight pass, avoiding 1x1 walls and taking into account object size
# Returns point closest to collision point and all passed points (intersected 1x1 squares)
function check_collisions(pos::Cind, dx::Rational, dy::Rational, robot_size::Rational, room)
    
    # dir is going to contain integer rounding of the path direction
    # We also specify rounding for 1//2: if it reaches square exactly, it can get there
    dir = Cind(Integer.(round.((dx + isinteger(2*dx)*sign(dx)//4,dy + isinteger(2*dy)*sign(dy)//4))))
    
    # If no move return immediately
    if (dir == Cind(0,0))
        return pos, [pos]
    end

    newpos = pos + dir
    coldist = Inf
    colpoints = Matrix{Vector{Rational}}(undef,abs(dir[1])+1,abs(dir[2])+1)
    for x = pos[1]:sign(dir[1])+(dir[1]==0):pos[1]+dir[1]
        for y = pos[2]:sign(dir[2])+(dir[2]==0):pos[2]+dir[2]
            dvec = Cind(x - pos[1], y - pos[2])
            # Sign of perpendicular direction to current point
            ds = sign(dvec[1]*dy-dvec[2]*dx)
            # Wall corner + robot size position
            cx = dvec[1] + sign(-dy*ds)*(1+robot_size)//2
            cy = dvec[2] + sign(dx*ds)*(1+robot_size)//2
            scal = cx*(-dy)*ds + cy*dx*ds
            # If corner is on the other side of the path, then square intersects path
            if scal >= 0
                # Check if collision happened earlier
                cx2 = dvec[1] - sign(dx)*(1+robot_size)//2
                cy2 = dvec[2] - sign(dy)*(1+robot_size)//2
                scal = cx2*(-dy)*ds + cy2*dx*ds
                # Check whether intersection is vertical or horizontal
                if ((cx2 == cx)*scal >= (cy2 == cy)*scal) && (dy != 0)
                    # Path intersects closest horizontal side
                    curpos = [cy2*dx//dy,cy2]
                else
                    # Path intersects closest vertical side
                    curpos = [cx2,cx2*dy//dx]
                end
                colpoints[abs(x-pos[1])+1,abs(y-pos[2])+1] = curpos

                # For finding first collision with wall (or room boundary):
                if !all(1 .<= (x,y) .<= size(room)) || (room[x,y] == wall)
                    if sum(abs.(curpos)) < coldist
                        coldist = sum(abs.(curpos))
                        # Specify rounding for 1//2: closer to path start
                        curpos -= (isinteger.(curpos*2)).*(sign.([dx,dy]))//4
                        newpos = pos + Cind(Tuple(Integer.(round.(curpos))))
                    end
                end
            else
                colpoints[abs(x-pos[1])+1,abs(y-pos[2])+1] = [Inf, Inf]
            end
        end
    end

    # Cycle again to find all passed squares (before collision)
    result = Vector{CartesianIndex}()
    for y = pos[2]:sign(dir[2])+(dir[2]==0):pos[2]+dir[2]
        for x = pos[1]:sign(dir[1])+(dir[1]==0):pos[1]+dir[1] 
            if (sum(abs.(colpoints[abs(x-pos[1])+1,abs(y-pos[2])+1])) <= coldist) && all(1 .<= (x,y) .<= size(room))
                # Prevent diagonal sight between walls for size 0 robot
                # In principle you can allow it, or already prevent sight when
                # one wall is present: whatever you think is reasonable
                if (abs(x-pos[1]) > abs(newpos[1]-pos[1])) && (abs(y-pos[2]) > abs(newpos[2]-pos[2]))
                    if (room[x-sign(dir[1]),y] == wall) && (room[x,y - sign(dir[2])] == wall)
                        continue
                    end
                end
                push!(result, Cind(x,y))
            end
        end
    end
    # Change order of x and y if necessary to create ordered result
    if (abs(dir[1]) > abs(dir[2]))
        sort!(result)
    end
    return newpos, result
end;

This function directtly interacts with the room and gives the robot all the necessary information: whether destination was reached, has it passed a goal etc.

# Move robot only if it can pass between square (1x1) walls moving in a straight line
# Automatically updates robot map using information about passed points
function attempt_move!(robot::Robot, dir::CartesianIndex, room)
    robot.pos, passed_points = check_collisions(robot.pos, dir, robot.size, room)
    dest_visited = false
    for point in passed_points
        # Mark all points before destination as visited
        if !dest_visited
            robot.visited[point] = true
        end
        if (point == robot.pos)
            dest_visited = true
        end
        # In case unknown goal was passed, stop there
        if (room[point] == goal) && (robot.map[point] != goal)
            robot.path = []
            robot.destination = point
            robot.map[point] = room[point]
            robot.pos = point
            break
        end
        # Update map
        robot.map[point] = room[point]
    end
    return
end;

As the function of collision checking can be hard to understand, here is a simple example, when robot is moving from one corner to the opposite corner of a small room.

Functions return the final point as well as all passed points in the order they were passed. Depending on robot's size, it can pass less or more squares and thus have less or more problems avoiding walls.

###############################################
# Illustration: robot moving from (1,1) to (2,5)
###############################################

testroom = zeros(Integer,2,5)
testroom[1:2,3] .= 1 # Wall in the middle of the path

# Large robot is going to collide with both walls and intersect more squares
println("Large robot observed:")
println(check_collisions(Cind(1,1),Cind(size(testroom))-Cind(1,1),1//2,testroom))

# Small robot is going to collide only with (1,3) wall and intersect less squares
println("Small robot observed:")
println(check_collisions(Cind(1,1),Cind(size(testroom))-Cind(1,1),1//10,testroom))

Large robot observed:
(CartesianIndex(1, 2), CartesianIndex[CartesianIndex(1, 1), CartesianIndex(1, 2), CartesianIndex(2, 2), CartesianIndex(1, 3), CartesianIndex(2, 3)])
Small robot observed:
(CartesianIndex(1, 2), CartesianIndex[CartesianIndex(1, 1), CartesianIndex(1, 2), CartesianIndex(1, 3)])

Sensors

Sensors are much easier, but also split into two parts.

First function returns a set of observed points in the room after calling all of the robot sensors.

Second function uses this data to update robot map and, in case sensors found the goal, also sets it as a new destination.

# Returns a list of points checked with sensors and their value
function check_sense(sensors, pos, dir, room)
    result = []
    for sensor in sensors
        points = sensor(pos, dir, room)
        for point in points
            push!(result, (point, room[point]))
        end
    end
    return result
end;

# Apply robot sensors
# In case unknown goal was sensed, set it as a new destination
function apply_sensors!(robot, room)
    sensor_data = check_sense(robot.sensors, robot.pos, robot.dir, room)
    for data in sensor_data
        if (data[2] == goal) && (robot.map[data[1]] != goal)
            robot.path = []
            robot.destination = data[1]
        end
        robot.map[data[1]] = data[2]
    end
    return
end;

Pathfinding

This is a standard pathfinding algorithm using breadth-first search. It is made more general by allowing arbitrary moves and remembering robot direction at each point.

using DataStructures; #for queue
function find_path!( robot::Robot, start::Cind, start_dir::Cind, goal )

    ####################
    # Initializing masks
    ####################
    
    wavefront = zeros( Integer, size(robot.map)...,  length(dirs)) # last index corresponds to robot direction
    visited = falses(size(robot.map)..., length(dirs))
    previous = Array{Tuple{Cind,Cind,Integer}}(undef,size(robot.map)..., length(dirs))
    
    ##############################################
    # Initializing starting location and direction
    ##############################################
    
    robot.path = []
    robot.destination = nothing
    queue = Queue{Vector{Cind}}()
    pos = start
    dir = start_dir
    enqueue!(queue, [pos, dir])
    visited[pos, dir_inds[dir]] = true

    ######################
    # Breadth-first search
    ######################

    # TO DO: can save distances: then when distances are equal, can, for example,
    # select path with more observed squares (using data from "check_sense")
    # or more observed squares with potential goals
    while length(queue) > 0
        pos, dir = dequeue!(queue)

        if (robot.map[pos] == goal) && !robot.visited[pos]
            robot.destination = pos
            break
        end

        current_depth = []
        for i = 1:length(robot.moves)
            newpos, newdir = robot.moves[i](pos,dir)
            newpos,_ = check_collisions(pos, newpos-pos, robot.size, robot.map)
            if (visited[newpos, dir_inds[newdir]] == false)
                enqueue!(queue, [newpos, newdir])
                visited[newpos, dir_inds[newdir]] = true
                previous[newpos, dir_inds[newdir]] = (pos, dir, i)
            end
        end
    end
    
    #####################
    # Path reconstruction
    #####################
    
    if (robot.destination != nothing)
        while (pos != start) || (dir != start_dir)
            oldpos, dir, move = previous[pos, dir_inds[dir]]
            push!(robot.path, (pos, move))
            pos = oldpos
        end
    end
    
    return
    
end;

WARNING: using DataStructures.reset! in module Main conflicts with an existing identifier.

Single action

Following function represents a single action of a robot: it first observes its surroundings using sensors, then tries to find a path (if not already found) to a goal or at least to a location, where goal could be, and, finally, attempts to make a move, following the path.

# Returns true if robot program is finished
# can interact with room only using "apply_sensors" and "attempt_move" functions
function action!( robot::Robot, room)

    apply_sensors!(robot, room)

    # If it already knows there is no goal at destination, find a new one
    if (robot.destination != nothing) && (robot.map[robot.destination] != goal)
        robot.path = []
        robot.destination = nothing
    end

    # If path is empty, compute it
    if length(robot.path) == 0
        # If destination exists, find path to it
        if (robot.destination != nothing)
            find_path!(robot,robot.pos,robot.dir,goal)
        end
        # If there is no path to destination or no destination, check possible goal locations
        if length(robot.path) == 0
            find_path!(robot,robot.pos,robot.dir,goal_check)
        end
        # If no path found again, no unvisited goals can be reached, so we output a stop signal
        if length(robot.path) == 0 
            return true
        end
    end
    
    pathpos, move = pop!(robot.path)
    newpos, newdir = robot.moves[move](robot.pos, robot.dir)
    robot.dir = newdir
    expectedpos,_ = check_collisions(robot.pos, newpos-robot.pos, robot.size, robot.map)

    # In principle, one can also save path points and check every time whether whole path is traversable
    # Here we only check that the current move is the same as predicted in the path, according to the known map
    if (expectedpos != pathpos)
        robot.path = []
        if robot.map[robot.destination] != goal
            robot.destination = nothing
        end
        return false
    end
    attempt_move!(robot, newpos-robot.pos, room)
    return false
end;

Main

This is the main function, which runs the robot and creates a gif animation out of studied maps after each robot action.

################################
# Main function to run the robot
################################

function run_robot!(robot, room)

    full_path = [ Tuple(robot.pos) ];

    a = Plots.Animation()

    plt = plot()

    for i = 1:600 # Limit gif to 1 minute (10 frames per second)
        result = action!( robot, room )

        push!(full_path, Tuple(robot.pos)) 
    
        plt = heatmap( -replace(robot.map', goal_check => 3, unknown => 3), yflip=:true, c=:greys, aspect_ratio=:equal, leg=:false )
        plot!( plt, first.(full_path), last.(full_path), aspect_ratio=:equal, lw=2, linez=range(0, stop=1, length=length(full_path)), c=:batlow )
        if length(robot.path) > 0 scatter!( plt, [robot.destination[1]], [robot.destination[2]], markersize=5, markerstrokewidth=5, markercolor=:red, shape=:xcross ); end;
    
        # Use red triangle of appropriate size to denote the robot
        if robot.dir == forward scatter!( plt, [robot.pos[1]], [robot.pos[2]], markersize=10, shape=:rtriangle, c=:red );
        elseif robot.dir == right scatter!( plt, [robot.pos[1]], [robot.pos[2]], markersize=7, shape=:dtriangle, c=:red );
        elseif robot.dir == backward scatter!( plt, [robot.pos[1]], [robot.pos[2]], markersize=10, shape=:ltriangle, c=:red );
        else scatter!( plt, [robot.pos[1]], [robot.pos[2]], markersize=7, shape=:utriangle, c=:red ); end;

        frame(a, plt)

        # If we don't stop when goal is reached, robot will continue to search for other goals and
        # try to reach them too
        if result || room[robot.pos] == goal
            break
        end
    end


    # Last frame is repeated for 50/10 = 5 seconds
    for i = 1:50
        frame(a, plt)
    end

    return gif( a, "robot_animation.gif", fps=10 )
end;

Универсальный робот