How to connect nodes for a* algorithm

See more:

trees

Python2.7

OpenCV

I am attempting to build a program where a user is able to select a starting and ending point on a map, and the shortest path between those two points is displayed. My code should work for all maps inputted, not just one.

I already have placed all the nodes on the map. I would like to connect these nodes in order to build a node tree, so I can perform the A* algorithm. I was wondering how might I code something that connects these nodes?

(eg: how does the computer know which nodes should be children of other nodes? How does the computer make sure that 2 nodes are not connected if there is an obstacle between them?) Thanks.


Image of all of the nodes on the map (note that no starting and ending point is displayed because they will be chosen each time by the user):

https://i.imgur.com/RMhSGoq.jpg[^]

Image of all the obstacles recognized on the map:

https://i.imgur.com/RpiY2tf.jpg[^]

What I have tried:

I have been stuck at a dead end here trying to solve the problem. I have not been able to come up with much in terms of solutions.


I currently have all of the nodes stored in an array, and all of the obstacles stored in another. Just for reference, I have included my code below, however, most of it concerns generating the nodes, and thus is irrelevant to the problem.

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Mar 15 21:23:40 2018

@author: 2020shatgiskesselldd
"""
import cv2
import numpy as np
import scipy.signal
import math


class Node:
    xcoordinate = 0;
    ycoordinate = 0;



roomimg = cv2.imread("/Users/2020shatgiskessell/Desktop/Maps/medium2.jpg")

# edge detection
# ret, thresh = cv2.threshold(roomimg, 127, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C)
thresh = cv2.cvtColor(roomimg, cv2.COLOR_BGR2GRAY)
edge = cv2.Canny(thresh, 100, 200)
height,width,channels = roomimg.shape

#define the dimensions of the grid
def estimate_noise(I):

  H, W = I.shape

  M = [[1, -2, 1],
       [-2, 4, -2],
       [1, -2, 1]]

  sigma = np.sum(np.sum(np.absolute(scipy.signal.convolve2d(np.array(I), M))))
  
  sigma = sigma * np.sqrt(0.5 * np.pi) / (6 * (W-2) * (H-2))

  return sigma

boxsize = (math.pow(estimate_noise(edge),-0.708)* 112.32)
matrix_icrement_width = int(width/int(boxsize))
matrix_icrement_height = int(height/int(boxsize))
Matrix =  [[0 for x in range(matrix_icrement_width)] for y in range(matrix_icrement_height)] 
Nodes = []

#defines what are obstacles and what are not
cut_off_point = 15
print (boxsize)
#U HAVE TO CHANGE CUT OFF POINT BASED ON EVERY IMAGE

box_num = 0
boxesdrawn = 0
for i in range (0,height, int(boxsize)):
    for j in range (0,width, int(boxsize)):
        #1. DRAW THE BLOCKS
        roi_gray = edge[i:i+int(boxsize),j:j+int(boxsize)]
        #2. FIND INTENSITY OF ROI
        roi_avg_intensity = np.mean(roi_gray)
        #3. BASED ON THAT, SEE IF ROI IS AN OBSTACLE OR NOT
        #print(box_num,"(",i,",",j,")")
        
        if roi_avg_intensity > cut_off_point:
            # if box_num < 200:
                # print("roi_avg_intensity:", roi_avg_intensity)
                
            #DRAW RECTANGLE AROUND OBSATCLE
            #cv2.rectangle(roomimg, (j,i), (j+int(boxsize), i+int(boxsize)),(0,0,255))
            Matrix[int(i/int(boxsize))][int(j/int(boxsize))] = "o"
            boxesdrawn += 1
        else:

            Matrix[int(i/int(boxsize))][int(j/int(boxsize))] = "c"
        box_num += 1
        
nodescreated = 0



def createNewNode(i,j):
    newNode = Node()
    newNode.xcoordinate = i
    newNode.ycoordinate = j

    Nodes.append(newNode)
    
    converted_j = j * int(boxsize)
    converted_i = i * int(boxsize)
    cv2.rectangle(roomimg, (converted_j,converted_i), (converted_j+int(boxsize), converted_i+int(boxsize)),(0,255,255))

#SO I KNOW THAT THE createNewNode(i,j) function is working as should
#AND I KNOW THAT THE Matrix DATA STRUCUTRE IS ALSO ALL G
#PERHAPS, INSTEAD OF USING A IMAGE, JUST USE A .TXT FILE AND ONLY DEAL WITH Matrix
def traverse_matrix():
    for i in range (0,matrix_icrement_width-1):
        for j in range (0,matrix_icrement_height-1):
            if Matrix[i][j]== "o" :
                #if u r on an obstacle, dont do anything
                continue
            
            if Matrix[i][j-2] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
            
            if Matrix[i][j-1] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
            
            if Matrix[i-2][j] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
            
            if Matrix[i-1][j] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
        
            if Matrix[i][j-1] == "o":
                #if u were on an obstacle, but not anymore
                createNewNode(i,j)
                Matrix[i][j] = "N"
            
            if Matrix[i+1][j] == "c":
                #if the space below u is a path
                createNewNode(i,j)
                Matrix[i][j] = "N"
                
            if Matrix[i][j+1] == "o":
                #if the space infront of u is an obstacle
                createNewNode(i,j)
                Matrix[i][j] = "N"

                

def printMatrix():
    f = open('obstaclemap.txt', 'w')
    f.write('\n'.join([''.join(['{:4}'.format(item) for item in row]) 
      for row in Matrix]))
    f.close()


def astar ():
    
#A STAR TIME
#1. Start with node N
    for node in Nodes:
        print (node.xcoordinate)
#2. Calculate distance between node N and its nearest neighbores in the NESW directions (possible add northeast, northwest, southeast, soutthwest)
#3. Calculate the distance of each of those nodes to the end point (the hueristic)
#4. Add them up
#5. Add the lowest cost node to the list
#6. N = lowest cost node
#FIGURE OUT WHAT TO DO AT END


traverse_matrix()
#printMatrix()
astar()

cv2.imwrite('roomimg.jpg', roomimg)
cv2.imshow("image", roomimg)


if cv2.waitKey(0) & 0xff == 27:
    cv2.destroyAllWindows()