Path planning for robot car on grid

Question

I am currently doing a project as my hobby. I am trying to simulate a robot car in a grid arena. Below is the image of the arena:

The start point of the car is the bottom-right corner(dark green box). And I have to reach at pink grid using the shortest path possible. Shortest path in the sense that cost of crossing the grid. The weights/costs of grids are as follows:
Red:4, Yellow:3, Green:2, White:1. Black grids are block/walls.

My implementation:

I used the OpenCV library in python to segment the colors of the grid and finding the centroids for each grid. centroid matrix is as follows:

[[( 34,  34) ( 85,  34) (137,  34) (  0,   0) (240,  34) (292,  34)]
 [( 34,  85) (  0,   0) (137,  85) (  0,   0) (240,  85) (291,  85)]
 [( 34, 137) ( 85, 137) (137, 137) (188, 137) (240, 137) (291, 137)]
 [(  0,   0) ( 86, 191) (137, 188) (188, 188) (  0,   0) (291, 188)]
 [( 34, 240) ( 85, 240) (137, 240) (  0,   0) (  0,   0) (291, 240)]
 [( 34, 292) (  0,   0) (137, 291) (188, 291) (240, 291) (291, 291)]]

Then I created an adjacency matrix(36*36) based on the nodes connected. Below is the adjacency matrix in form of a list:

[[0 2 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [1 0 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 2 0 0 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 3 0 0 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [1 0 0 0 0 0 0 0 0 0 0 0 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 4 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 3 0 0 0 0 0 0 2 0 0 0 0 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 2 0 0 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 3 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 2 0 0 0 0 2 0 4 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 3 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 4 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 2 0 0 0 0 3 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 2 0 0 0 0 0 0 2 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 4 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 2 0 1 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 0 0 0 0 1 0 0 0 0 0 0 4 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 1]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 2 0 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 4 0 2 0]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 0 1]
 [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 2 0]]

Then I have implemented the Dijkstra algorithm to find the shortest path as:

from collections import defaultdict
class Graph:
    def minDistance(self,dist,queue):
        minimum = float("Inf")
        min_index = -1
        for i in range(len(dist)):
            if dist[i] < minimum and i in queue:
                minimum = dist[i]
                min_index = i
        return min_index
def printPath(self, parent, j):
    if parent[j] == -1 : 
        print (j)
        return
    self.printPath(parent , parent[j])
    print(j)
def printSolution(self, dist, parent):
    src = 0
    print("Vertex \t\tDistance from Source\tPath")
    for i in range(1, len(dist)):
        print("\n%d --> %d \t\t%d \t\t\t\t\t" % (src, i, dist[i])),
        self.printPath(parent,i)
def dijkstra(self, graph, src):

    row = len(graph)
    col = len(graph[0])
    dist = [float("Inf")] * row
    parent = [-1] * row
    dist[src] = 0
    queue = []
    for i in range(row):
        queue.append(i)
    while queue:
        u = self.minDistance(dist,queue)  
        queue.remove(u)
    for i in range(col):
        if graph[u][i] and i in queue:
            if dist[u] + graph[u][i] < dist[i]:
                dist[i] = dist[u] + graph[u][i]
                parent[i] = u
    self.printSolution(dist,parent)

However, this code is raising an error on executing as:

g= Graph()
g.dijkstra(graph,35) #src at node 35    

Error:

---------------------------------------------------------------------------
  ValueError                                Traceback (most recent call last)
<ipython-input-166-4785fe512432> in <module>
      1 g= Graph()
      2 graph = adj
----> 3 g.dijkstra(graph,35)

<ipython-input-146-4fd492228481> in dijkstra(self, graph, src)
     86 
     87             # remove min element
---> 88             queue.remove(u)
     89 
     90             # Update dist value and parent

 ValueError: list.remove(x): x not in list

The output should be like this:

 Vertex           Distance         Path 
 0 -> 2          12        0 1 2 

My doubts/questions:

1) Am I implementing/proceeding rightly to help the car reach its destination? Are these ideas good or I should choose a different approach? If yes, please suggest.

2) How can I resolve the issue in my code? I don't know why I am getting this error. If I should use a different algorithm/approach, please sugget.

3) Is my adjacency matrix right? I made it based on 36 connecting nodes with weights.

I am not a computer science student but doing it because of my interest. I have got no guide/teacher to teach me these things and looking at this site to resolve my issues. I request you to please provide guidance.
Thank you.

Answer 1

I don't have time to do a full code review here, but the short version of why you're getting the error is that, as the error states, you're trying to remove an element from queue that either wasn't added or was already removed.

I don't think your code is doing what you think it's doing, so I would highly suggest you attach a debugger and step through your code and watch it operate. For example:

def minDistance(self,dist,queue):
    minimum = float("Inf")
    min_index = -1
    for i in range(len(dist)):
        if dist[i] < minimum and i in queue:
            minimum = dist[i]
            min_index = i
    return min_index

You're comparing all the entries in dist to float("Inf"), but what is dist? That's defined in the calling function:

def dijkstra(self, graph, src):

    dist = [float("Inf")] * row

So if every element in dist is float("Inf"), then will there ever be an element in dist that is less than float("Inf")? No. So what happens then, if your distance entries aren't less than Inf? You will still

return min_index

where min_index is initialized to -1.

The summary on what I think is going wrong here is this:

1. You build a queue of all rows, then
2. You check the distance on each row, then
3. If that distance fails your check, you are returning -1 as the index, then
4. You are not checking the index on the returned variable, you're instead just blindly removing that index from queue, then
5. Because queue is a vector of row numbers and -1 is not a row number, you get an exception when you try to remove -1 from your queue.

The part that I don't have time to get into here is your intent. Why are you trying to do with your code? You are running

while queue:
    u = self.minDistance(dist,queue)  
    queue.remove(u)

This will remove all the entries in your queue until queue is null/empty, but as mentioned above self.minDistance will return -1 unless the dist[i] < minimum and i in queue.

I can't tell what your intent is, but I think maybe you should just be calling u = self.minDistance once and not repeatedly. This hunch seems to be further confirmed by looking a few lines down in your code:

while queue:
    u = self.minDistance(dist,queue)  
    queue.remove(u)
for i in range(col):
    if graph[u][i] and i in queue:

If you are running queue.remove(u) until queue becomes empty/null, then how does it exist later when you check if graph[u][i] and i in queue?

I would guess instead of:

for i in range(row):
    queue.append(i)
while queue:
    u = self.minDistance(dist,queue)  
    queue.remove(u)
for i in range(col):
    if graph[u][i] and i in queue:

would you would want

for i in range(row):
    queue.append(i)
u = self.minDistance(dist,queue)  
for i in range(col):
    if graph[u][i] and i in queue:

but I'm not a python user and haven't looked more at your algorithm.

At a minimum, though, if you are going to do queue.remove(u) then you need to check if queue contains u first. If it doesn't then print yourself a nice warning and I'd bet that'll reveal your issue.