Path planning for robot car on grid

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()
----> 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.

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?

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.

• thank you for your kind answer. I thought removing 'u' from the queue and saving it to the parent list might work. Cause once I have included that node in the path of the car, it's not necessary to iterate on it('u') again to get the new nodes with a minimum distance of current node. However, I will implement your advice to resolve the issue. Thanks!
– UJM
Mar 29 at 14:47
• @UjjawalM. - As I stated in the post, I haven't looked hard at your methods beyond the particular error, so I could be wrong here with my guidance, but it looks like you're iterating through all the entries in dist in the minDistance function, so I'm not sure what is going to happen on subsequent calls that would get you lower distances. You don't appear to be recalculating anything that I can see; you just remove an entry from queue and do the same thing over again. Hope this helps! Mar 29 at 17:16