Navigating through a Maze using path-planning (Dijkstra)

Question

I'm working on an robot that would be able to navigate through a maze, avoid obstacles and identify some of the objects in it. I have a monochromatic bitmap of the maze, that is supposed to be used in the robot navigation.

Up till now I have processed the bitmap image, and converted it into an adjacency list. I will now use the dijkstra's algorithm to plan the path.

However the problem is that I have to extract the entrance point/node and exit node from the bmp image itself for dijkstra's algorithm to plan the path.

The robots starting position will be slightly different (inch or two before the entrance point) from the entrance point of maze, and I am supposed to move to the entrance point using any "arbitrary method" and then apply dijkstra algorithm to plan path from maze's entrance to exit.

On the way I have to also stop at the "X's" marked in the bmp file I have attached below. These X's are basically boxes in which I have to pot balls. I will plan the path from entrance point to exit point , and not from the entrance to 1st box, then to second, and then to the exit point; because I think the boxes will always be placed at the shortest path.

Since the starting position is different from the entrance point, how will I match my robot's physical location with the coordinates in the program and move it accordingly. Even if the entrance position would have been same as starting position there may have been an error. How should I deal with it? Should I navigate only on the bases of the coordinates provided by dijkstra or use ultrasonics as well to prevent collisions? And if we yes, can you give me an idea of how should I use the both (ultrasonics, and coordinates)?

Here's the sample Bitmap image of the maze.

Answer 1

I know you need this for robotics but here is an example how to translate pixels to array in java to give some ideas?

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.SwingUtilities;
import javax.swing.Timer;
import javax.swing.WindowConstants;

public class RobotDemo extends JFrame {
    private static final long serialVersionUID = 1L;


    public RobotDemo() {
        super("Robot Demo");
        setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE);
        getContentPane().add(new RobotPanel(), BorderLayout.CENTER);
        pack();
        setResizable(false);
        setLocationRelativeTo(null);        
    }

    public static void main(String[] args) {
        SwingUtilities.invokeLater(new Runnable() {
            public void run() {
                JFrame frame = new RobotDemo();
                frame.setVisible(true);
            }
        });     
    }
}

interface Constants {

    public static final int TILE_WIDTH = 32;
    public static final int TILE_HEIGHT = 32;
    public static final int NUM_TILE_COLS = 20;
    public static final int NUM_TILE_ROWS = 10;
    public static final int PIXEL_STEPS = 3;
    public static final int REFRESH_RATE = 200;
    public static final Dimension PANEL_SIZE = new Dimension(TILE_WIDTH * NUM_TILE_COLS, TILE_HEIGHT * NUM_TILE_ROWS);  

    public static enum RobotState {
        awaiting_instruction,
        moving_north,
        moving_south,
        moving_east,
        moving_west     
    };

    public static enum RobotInstruction {
        NORTH,
        SOUTH,
        EAST,
        WEST    
    }

    public void draw(Graphics g);
}

class RobotPanel extends JPanel implements Constants, ActionListener {
    private static final long serialVersionUID = 1L;    

    private Timer timer = new Timer(REFRESH_RATE, this);
    private Map map = new Map();
    private Robot robot = new Robot(map);

    public RobotPanel() {
        timer.start();
    }

    public Dimension getPreferredSize() { return PANEL_SIZE; }
    public Dimension getMinimumSize() { return PANEL_SIZE; }
    public Dimension getMaximumSize() { return PANEL_SIZE; }

    protected void paintComponent(Graphics g) {
        super.paintComponent(g);        
        map.draw(g);
        robot.draw(g);
        draw(g);
    }

    public void actionPerformed(ActionEvent e) {
        robot.update(); 
        repaint();
    }

    public void draw(Graphics g) {
        for(int r = 0; r < NUM_TILE_ROWS; r++) {
            for(int c = 0; c < NUM_TILE_COLS; c++) {
                g.drawRect(c * TILE_WIDTH, r * TILE_HEIGHT, TILE_WIDTH, TILE_HEIGHT);
            }
        }               
    }
}

class Robot implements Constants {

    private RobotState state = RobotState.moving_east;
    private int row = TILE_HEIGHT;
    private int col = TILE_WIDTH;

    private int mapX = 1;
    private int mapY = 1;
    private Map map;

    int nextRowCheck = 1;
    int nextColCheck = 2;

    public Robot(Map m) {
        map = m;
    }

    public int getRow() {
        return mapY;
    }

    public int getCol() {
        return mapX;
    }

    private boolean needsNewInstruction(){
        int newRow = row;
        int newCol = col;

        if(state == RobotState.moving_north) newRow -= PIXEL_STEPS;
        if(state == RobotState.moving_south) newRow += PIXEL_STEPS;
        if(state == RobotState.moving_east) newCol += PIXEL_STEPS;
        if(state == RobotState.moving_west) newCol -= PIXEL_STEPS;

        if((newRow / TILE_HEIGHT) != mapY) return true;
        if((newCol / TILE_WIDTH) != mapX) return true; 

        return false;
    }


    public void draw(Graphics g) {
        Color c = g.getColor();
        g.setColor(Color.GREEN);
        g.fillRect(col, row, TILE_WIDTH, TILE_HEIGHT);
        g.setColor(c);
    }

    public void update() {

        System.out.println("UPDATE [" + row + "][" + col + "] = [" + (row / TILE_HEIGHT) + "][" + (col / TILE_WIDTH) + "]");


        if(needsNewInstruction()) {
            System.out.println("NEEDS NEW INSTRUCTION [" + row + "][" + col + "] = [" + (row / TILE_HEIGHT) + "][" + (col / TILE_WIDTH) + "]");

            mapX = nextColCheck;
            mapY = nextRowCheck;
            System.out.println("UPDATED MAP REFERENCE [" + mapY + "][" + mapX  + "]");

            row = mapY * TILE_HEIGHT;
            col = mapX * TILE_WIDTH;
            System.out.println("UPDATED PIXEL REFERENCE [" + row + "][" + col  + "]");

            RobotInstruction instruction = map.getNextInstruction(this);

            if(instruction == RobotInstruction.NORTH) {
                state = RobotState.moving_north;
                nextRowCheck = mapY - 1;
            }
            if(instruction == RobotInstruction.SOUTH) {
                state = RobotState.moving_south;
                nextRowCheck = mapY + 1;
            }
            if(instruction == RobotInstruction.EAST) {
                state = RobotState.moving_east;
                nextColCheck = mapX + 1;
            }
            if(instruction == RobotInstruction.WEST) {
                state = RobotState.moving_west;
                nextColCheck = mapX - 1;
            }
        }
        move();
    }

    public void move() {
        if(state == RobotState.moving_north) row -= PIXEL_STEPS;
        if(state == RobotState.moving_south) row += PIXEL_STEPS;
        if(state == RobotState.moving_east) col += PIXEL_STEPS;
        if(state == RobotState.moving_west) col -= PIXEL_STEPS;
    }
}

class Map implements Constants {
    int[][] map = new int[][] {
            {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
            {1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 1, 1, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},           
            {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
    };  

    public Map() {

    }

    public RobotInstruction getNextInstruction(Robot robot) {
        int row = robot.getRow();
        int col = robot.getCol();

        System.out.println("GET NEXT INSTRUCTION FOR [" + row + "][" + col + "]");

        if(map[row][col + 1] == 0) return RobotInstruction.EAST;
        if(map[row + 1][col] == 0) return RobotInstruction.SOUTH;
        if(map[row - 1][col] == 0) return RobotInstruction.NORTH;
        if(map[row][col - 1] == 0) return RobotInstruction.WEST;

        return null;
    }

    public void draw(Graphics g) {
        Color color = g.getColor();
        for(int r = 0; r < NUM_TILE_ROWS; r++) {
            for(int c = 0; c < NUM_TILE_COLS; c++) {
                g.setColor(map[r][c] == 0 ? Color.CYAN : Color.RED);
                g.fillRect(c * TILE_WIDTH, r * TILE_HEIGHT, TILE_WIDTH, TILE_HEIGHT);
            }
        }
        g.setColor(color);
    }
}

Here is an example how to populate your navigational array with directions. the code above doesn't use the code below so you would have to do that yourself ...

public class Maze {

    private static final char E = 'E';   // Ending position
    private static final char X = 'X';   // Wall
    private static final char O = ' ';   // Space
    private static final char L = 'L';   // Left
    private static final char R = 'R';   // Right
    private static final char U = 'U';   // Up
    private static final char D = 'D';   // Down

    private static final char FALSE = '0';   // Not accessible
    private static final char TRUE = '1';    // Is accessible
    private static final Node END_NODE = new Node(4, 4);

    private static final int[] ROW_DIRECTIONS = {-1, 1,  0, 0};  
    private static final int[] COL_DIRECTIONS = { 0, 0, -1, 1};
    private static final char[][] OPPOSITES = new char[][] {{O, D, O},{R, O, L},{O, U, O}};

    public static void main(String[] args) {

        char[][] maze = new char[][] { 
            {X, X, X, X, X, X},
            {X, O, O, X, O, X},
            {X, O, X, X, O, X},
            {X, O, O, O, X, X},
            {X, X, O, X, O, X},
            {X, O, O, O, O, X},
            {X, O, X, X, O, X},
            {X, X, X, X, X, X}};

        // PLOT THE DESTINATION CELL AND ADD IT TO LIST
        List<Node> nodes = new ArrayList<Node>();
        nodes.add(END_NODE);
        maze[END_NODE.row][END_NODE.col] = E;

        // PRINT THE MAZE BEFORE ANY CALCULATIONS
        printMaze(maze);

        // SOLVE THE MAZE
        fillMaze(maze, nodes);
        printMaze(maze);

        // CONVERT MAZE TO AN ADJACENCY MATRIX
        compileMaze(maze);
        printMaze(maze);
    }

    /**
     * The parallel arrays define all four directions radiating from
     * the dequeued node's location. 
     * 
     * Each node will have up to four neighboring cells; some of these
     * cells are accessible, some are not. 
     * 
     * If a neighboring cell is accessible, we encode it with a directional
     * code that calculates the direction we must take should we want to 
     * navigate to the dequeued node's location from this neighboring cell.
     * 
     * Once encoded into our maze, this neighboring cell is itself queued 
     * up as a node so that recursively, we can encode the entire maze.
     */
    public static final void fillMaze(char[][] maze, List<Node> nodes) {

        // dequeue our first node
        Node destination = nodes.get(0);
        nodes.remove(destination);

        // examine all four neighboring cells for this dequeued node
        for(int index = 0; index < ROW_DIRECTIONS.length; index++) {
            int rowIndex = destination.row + ROW_DIRECTIONS[index];
            int colIndex = destination.col + COL_DIRECTIONS[index];

            // if this neighboring cell is accessible, encode it and add it
            // to the queue
            if(maze[rowIndex][colIndex] == O) {
                maze[rowIndex][colIndex] = getOppositeDirection(ROW_DIRECTIONS[index], COL_DIRECTIONS[index]);
                nodes.add(new Node(rowIndex, colIndex));
            }
        }
        // if our queue is not empty, call this method again recursively 
        // so we can fill entire maze with directional codes
        if(nodes.size() > 0) {
            fillMaze(maze, nodes);
        }
    }

    /**
     * Converts the maze to an adjacency matrix. 
     */
    private static void compileMaze(char[][] maze) {
        for(int r = 0; r < maze.length; r++) {
            for(int c = 0; c < maze[0].length; c++) {
                if(maze[r][c] == X || maze[r][c] == O) {
                    maze[r][c] = FALSE;
                }
                else {
                    maze[r][c] = TRUE;
                }
            }
        }
    }

    /**
     * prints the specified two dimensional array 
     */
    private static final void printMaze(char[][] maze) {
        System.out.println("====================================");
        for(int r = 0; r < maze.length; r++) {
            for(int c = 0; c < maze[0].length; c++) {
                System.out.print(maze[r][c] + "  ");
            }
            System.out.print("\n");
        }
        System.out.println("====================================");
    }

    /**
     * Simply returns the opposite direction from those specified 
     * by our parallel direction arrays in method fillMaze. 
     * 
     * coordinate 1, 1 is the center of the char[][] array and 
     * applying the specified row and col offsets, we return the
     * correct code (opposite direction)
     */
    private static final char getOppositeDirection(int row, int col) {
         return OPPOSITES[1 + row][1 + col];
    }
}

class Node {
    int row;
    int col;

    public Node(int rowIndex, int colIndex) {
        row = rowIndex;
        col = colIndex;
    }
}