def chk_intersection(req):
line_obstacles, pts = make_obstacles_scan(req.scan_list)
#print(line_obstacles)
for obstacle in line_obstacles:
print(
check_intersection_scan([req.start_pos.point, req.goal_pos.point],
line_obstacles))
if check_intersection_scan([req.start_pos.point, req.goal_pos.point],
line_obstacles):
print("Found Intersection")
return DynamicResponse(False)
print("No Intersection")
return DynamicResponse(True)
def __call__(self, goal_point, scan, start_point=[0, 0], animation=True):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
scan: list of obstacles which themselves are list of points
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
search_until_max_iter = True
# Make line obstacles and scan in x,y from scan
line_obstacles, pts = make_obstacles_scan(scan)
# Setting Start and End
self.start = Node(start_point[0], start_point[1])
self.goal = Node(goal_point[0], goal_point[1])
# Initialize node with Starting Position
self.node_list = [self.start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
########################################
# print("ITER-->")
# print(iter)
########################################
#########################################
# print("NODE_LIST-->")
# for printer_i in self.node_list:
# print(printer_i)
#########################################
# Sample a Random point in the sample area
rnd_point = sampler(self.sample_area, (self.goal.x, self.goal.y),
self.goal_sample_rate)
########################################
# print("RANDOM POINT-->")
# print(rnd_point)
########################################
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in self.node_list
]
nearest_node = self.node_list[distance_list.index(
min(distance_list))]
########################################
# print("NEAREST_NODE-->")
# print(nearest_node.x , nearest_node.y , nearest_node.cost)
########################################
# Creating a new Point in the Direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
#########################################
# print("NEW_POINT-->")
# print(new_point[0],new_point[1])
#########################################
# Check obstacle collision
new_point = scan_obstacle_checker(scan, new_point)
if math.isnan(new_point[0]):
########################################
#print("ISNAN-->"+str(iter))
# print(new_point)
########################################
continue
#If iterations is less than certain no. try exploring a bit
if iter < 20:
new_node = Node(new_point[0], new_point[1])
new_node.parent = nearest_node
new_node.cost = nearest_node.cost + math.sqrt(
(new_node.x - nearest_node.x)**2 +
(new_node.y - nearest_node.y)**2)
#Set the path for new node
present_node = new_node
px = []
py = []
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
new_node.path_x = px[:]
new_node.path_y = py[:]
self.node_list.append(new_node)
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
continue
nearest_indexes = find_near_nodes(self.node_list, new_point,
self.circle)
# Getting the parent node from nearest indices
costs = [
] # List of Total costs from the start to new_node when attached to parent node in node_list
temp_points = []
for index in nearest_indexes:
near_node = self.node_list[index]
point_list = [(near_node.x, near_node.y),
(new_point[0], new_point[1])]
if not check_intersection_scan(point_list, line_obstacles):
costs.append(near_node.cost +
math.sqrt((near_node.x - new_point[0])**2 +
(near_node.y - new_point[1])**2))
else:
costs.append(float("inf"))
min_cost = min(costs)
# Calculating the minimum cost and selecting the node for which it occurs as parent child
if min_cost == float("inf"):
continue
# Setting the new node as the one with min cost
min_ind = nearest_indexes[costs.index(min_cost)]
new_node = Node(new_point[0], new_point[1])
new_node.parent = self.node_list[min_ind]
new_node.cost = min_cost
#########################################
# print("NEW_NODE-->")
# print(new_node.x , new_node.y , new_node.cost)
#########################################
if new_node:
self.node_list.append(new_node)
for ind in nearest_indexes:
node_check = self.node_list[ind]
point_list = [(new_node.x, new_node.y),
(node_check.x, node_check.y)]
no_coll = not check_intersection_scan(
point_list, line_obstacles)
cost_improv = new_node.cost + math.sqrt(
(new_node.x - node_check.x)**2 +
(new_node.y - node_check.y)**2) < node_check.cost
if no_coll and cost_improv:
node_check.parent = new_node
present_node = new_node
px = []
py = []
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
new_node.path_x = px[:]
new_node.path_y = py[:]
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
if (not search_until_max_iter
) and new_node: # check reaching the goal
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
return None
def __call__(self, start_point, goal_point, scan, animation=False):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
scan_list:LaserScan polar distances to Obstacles [r1,r2,r3...] initially assuming every scan occurs at 1 rad interval
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
#Make line obstacles and scan in x,y from scan_list
line_obstacles, pts = make_obstacles_scan(scan)
# Initialize start and goal nodes
start = Node.from_coordinates(start_point)
goal_node = Node.from_coordinates(goal_point)
# Initialize node_list with start
node_list = [start]
# Calculate distances between start and goal
del_x, del_y = start.x - goal_node.x, start.y - goal_node.y
distance_to_goal = math.sqrt(del_x**2 + del_y**2)
# Loop to keep expanding the tree towards goal if there is no direct connection
if check_intersection_scan([start_point, goal_point], line_obstacles):
while True:
# Sample random point in specified area
rnd_point = self.sampler(self.sample_area, goal_point,
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in node_list
]
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
# Check whether new point is inside an obstacles
new_point = scan_obstacle_checker(scan, new_point)
# Expand tree
if math.isnan(new_point[0]):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
del_x, del_y = new_node.x - goal_node.x, new_node.y - goal_node.y
distance_to_goal = math.sqrt(del_x**2 + del_y**2)
if distance_to_goal < self.expand_dis or not check_intersection_scan(\
[new_node.to_tuple(), goal_node.to_tuple()], line_obstacles):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start
node_list = [start, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while node_list[node_list.index(last_node)].parent is not None:
node = node_list[node_list.index(last_node)]
path.append(node.to_tuple())
last_node = node.parent
path.append(start.to_tuple())
if animation == True:
RRT.visualize_tree(node_list, obstacle_list)
return path, node_list
def test1():
inf = np.nan_to_num(np.inf)
# ====Search Path with RRT*====
scan_list = [
inf, inf, inf, inf, inf, inf, inf, 3.056861639022827,
3.0252268314361572, 3.0203325748443604, 2.0132949352264404,
1.9735280275344849, 1.953012466430664, 1.9486260414123535,
1.9373271465301514, 1.9421530961990356, 1.9569212198257446,
2.002772331237793, inf, inf, inf, 0.9855800271034241,
0.9726546406745911, 0.9509224891662598, 0.9256065487861633,
0.9257093071937561, 0.922886073589325, 0.9183664917945862,
0.906120240688324, 0.8961981534957886, 0.9056581854820251,
0.9420517683029175, 0.9265859723091125, 0.9183934330940247,
0.9498110413551331, 0.9667358994483948, 1.0045623779296875,
2.442192792892456, 2.4326982498168945, 2.4553067684173584,
2.4645442962646484, 2.506408214569092, inf, inf, inf, inf, inf, inf,
inf, inf, inf, inf, inf, inf, inf, inf, 1.7533923387527466,
1.71872878074646, 1.70149827003479, 1.6858848333358765,
1.6909033060073853, 1.7164965867996216, 1.7079846858978271,
1.7376644611358643, 1.785688877105713, 3.3595521450042725,
3.3191449642181396, 3.29377818107605, 3.2705166339874268,
3.2538671493530273, 3.2152647972106934, 3.2159855365753174,
3.19524884223938, 3.0854270458221436, 2.8649425506591797,
2.6712183952331543, 2.554349184036255, 2.556751251220703,
2.5452141761779785, 2.5436341762542725, 2.5213427543640137,
2.5272300243377686, 2.5183942317962646, 2.509161949157715,
2.5124869346618652, 2.4297120571136475, 2.3386430740356445,
2.2512683868408203, 2.1818249225616455, 2.103046417236328,
2.0564870834350586, 1.9878475666046143, 1.9268088340759277,
1.885642170906067, 1.8311305046081543, 1.7829922437667847,
1.7410887479782104, 1.7179292440414429, 1.65846848487854,
1.6332777738571167, 1.5900510549545288, 1.5677014589309692,
1.5201916694641113, 1.4969840049743652, 1.4841266870498657,
1.4385578632354736, 1.4428545236587524, 1.4075186252593994,
1.3866387605667114, 1.3508318662643433, 1.3370169401168823,
1.3260891437530518, 1.3067781925201416, 1.2939659357070923,
1.2805120944976807, 1.2707873582839966, 1.2281290292739868,
1.2153609991073608, 1.2014520168304443, 1.2100038528442383,
1.1812770366668701, 1.1618354320526123, 1.1519147157669067,
1.1456130743026733, 1.1571439504623413, 1.1326026916503906,
1.1386666297912598, 1.0997953414916992, 1.091562271118164,
1.1005403995513916, 1.1073905229568481, 1.075814962387085,
1.0699113607406616, 1.078955054283142, 1.0810444355010986,
1.0660544633865356, 1.0604965686798096, 1.0380046367645264,
1.0327768325805664, 1.078073501586914, 1.0491496324539185,
1.0314854383468628, 1.0256805419921875, 1.0399309396743774,
1.039351224899292, 1.0458898544311523, 1.0328317880630493,
1.0365073680877686, 1.0345937013626099, 1.0336904525756836,
1.045121431350708, 1.019679069519043, 0.9782207012176514,
0.9893175959587097, 0.9512673616409302, 0.9016335606575012,
0.8861187696456909, 0.8714970350265503, 0.8524404168128967,
0.8410521745681763, 0.8306225538253784, 0.806861162185669,
0.7750387787818909, 0.772260308265686, 0.7613915205001831,
0.7231266498565674, 0.7228431701660156, 0.7109572291374207,
0.7123162746429443, 0.6959463357925415, 0.6842755079269409,
0.6778551936149597, 0.672926127910614, 0.6455686688423157,
0.6492228507995605, 0.650477945804596, 0.6205228567123413,
0.63560950756073, 0.6331012845039368, 0.6159425973892212,
0.6135613918304443, 0.5959789752960205, 0.610405445098877,
0.5870363712310791, 0.5804761052131653, 0.6007770299911499,
0.5676383972167969, 0.5660935640335083, 0.5781680345535278,
0.5915207862854004, 0.5583205819129944, 0.5651285648345947,
0.5652458071708679, 0.5596476197242737, 0.545867919921875,
0.5507373809814453, 0.5318761467933655, 0.542111337184906,
0.540361225605011, 0.5328619480133057, 0.5332945585250854,
0.5352373123168945, 0.5453101992607117, 0.5408035516738892,
0.5488573908805847, 0.5210607647895813, 0.5293474793434143,
0.5348880290985107, 0.5456039905548096, 0.5199136734008789,
0.5178253650665283, 0.5266040563583374, 0.5523934364318848,
0.5342961549758911, 0.5384075045585632, 0.5334569215774536,
0.5281636714935303, 0.5111650228500366, 0.5428661704063416,
0.5401403903961182, 0.5570170879364014, 0.5265321731567383,
0.5520188212394714, 0.5388387441635132, 0.5361006855964661,
0.5432685613632202, 0.5588036775588989, 0.5637043118476868,
0.5593273043632507, 0.5618724226951599, 0.5681909918785095,
0.580647349357605, 0.5878729224205017, 0.567261815071106,
0.5693681240081787, 0.5898579955101013, 0.6078497171401978,
0.6051531434059143, 0.6091262102127075, 0.5822492837905884,
0.6195617318153381, 0.6141515374183655, 0.617192804813385,
0.6386907696723938, 0.6485453248023987, 0.6483967900276184,
0.6659299731254578, 0.6548660397529602, 0.6864899396896362,
0.6838104128837585, 0.6932967901229858, 0.7090590596199036,
0.7026281356811523, 0.7346917986869812, 0.7633113861083984,
0.7599661350250244, 0.7832435369491577, 0.779472291469574,
0.8087064623832703, 0.8164687752723694, 0.8538014888763428,
0.8430877923965454, 0.8757237195968628, 0.8956906199455261,
0.9206846356391907, 0.9354161024093628, 0.9733413457870483,
0.9885174036026001, 1.0486935377120972, 1.049687385559082,
1.1021144390106201, 1.1006282567977905, 1.1586135625839233,
1.2052730321884155, 1.2459439039230347, 1.302675485610962,
1.3653827905654907, 1.4164248704910278, 1.4472814798355103,
1.535380244255066, 1.532191514968872, 1.5412589311599731,
1.5219082832336426, 1.5472310781478882, 1.5644363164901733,
1.55789053440094, 1.5711780786514282, 1.5786079168319702,
1.5898356437683105, 1.600523829460144, 1.6128002405166626,
1.6159331798553467, 1.626638412475586, 1.8782384395599365,
2.235314130783081, 2.2563958168029785, 2.2954134941101074,
2.307398557662964, 2.3106799125671387, 2.351066827774048,
2.391575813293457, 2.3959853649139404, 2.430370569229126,
2.449401378631592, 2.478189468383789, 2.516754150390625,
2.5472171306610107, 2.59478759765625, 2.6314077377319336,
2.6275601387023926, 2.682375907897949, 2.7329845428466797,
2.7940165996551514, 2.8262627124786377, 2.891613245010376,
2.943937301635742, 2.9711005687713623, 3.0209319591522217,
3.1053292751312256, 1.0324296951293945, 1.0091277360916138,
1.0003530979156494, 0.9934810996055603, 0.9784740209579468,
0.9725538492202759, 0.9597940444946289, 0.9535285830497742,
0.9423716068267822, 0.9491086602210999, 0.966484010219574,
0.9733442068099976, 0.9970974326133728, 1.0060924291610718,
1.0158592462539673, 1.0913983583450317, inf, inf, inf, inf,
2.0314512252807617, 2.0178802013397217, 1.9728460311889648,
1.961500883102417, 1.9714338779449463, 1.9641704559326172,
2.0064427852630615, 2.0385403633117676, 3.066349506378174,
3.039686918258667, 3.0638043880462646, 3.07513165473938, inf, inf, inf,
inf, inf, inf, inf, inf, inf
]
# Set Initial parameters
rrt_star = RRTStar(sample_area=[-5, 5], search_until_max_iter=True)
print('\n ' + '-' * 30 + "\n> Starting operation ...\n " + '-' * 30 + '\n')
start_time = time.time()
path = rrt_star(goal_point=[0, -4], scan=scan_list)
print('\n ' + '-' * 30 +
"\n> Time taken: {:.4} seconds.\n ".format(time.time() -
start_time) + '-' * 30 +
'\n')
if path is None:
print("Cannot find path")
else:
print("found path!!")
show_animation = True
# Draw final path
if show_animation:
rrt_star.draw_graph(scan_list)
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.show()
line_obs, pts = make_obstacles_scan(scan_list)
print('\n ' + '-' * 30 + "\n> Starting operation ...\n " + '-' * 30 +
'\n')
start_time = time.time()
optimized_path = path_optimizer(path, line_obs)
print("Path optimized.")
print('\n ' + '-' * 30 +
"\n> Time taken: {:.4} seconds.\n ".format(time.time() -
start_time) +
'-' * 30 + '\n')
# # Visualize path
visualize_scan(optimized_path, scan_list)
def __call__(self, goal_point, scan, start_point=[0, 0], animation=False):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
scan: list of obstacles which themselves are list of points
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
# Make line obstacles and scan in x,y from scan
line_obstacles, _ = make_obstacles_scan(scan)
# Setting Start and End
self.start = Node(start_point[0], start_point[1])
self.goal = Node(goal_point[0], goal_point[1])
# Initialize node with Starting Position
self.node_list = [self.start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
# Sample a Random point in the sample area
rnd_point = sampler(self.sample_area, (self.goal.x, self.goal.y),
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in self.node_list
]
nearest_node = self.node_list[distance_list.index(
min(distance_list))]
# Creating a new Point in the Direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
# Check obstacle collision
new_point = scan_obstacle_checker(scan, new_point)
if math.isnan(new_point[0]):
continue
###############################################################################################################
#THIS WILL ONLY WORK FOR SOME INITIAL ITERATIONS
#If iterations is less than certain no. try exploring a bit, run similar to RRT
if iter < self.initial_explore:
new_node = Node(new_point[0], new_point[1])
new_node.parent = nearest_node
new_node.cost = nearest_node.cost + math.sqrt(
(new_node.x - nearest_node.x)**2 +
(new_node.y - nearest_node.y)**2)
#Set the path for new node
present_node = new_node
px = [] #X-coordinate path
py = [] #Y-coordinate path
#Keep on appending path until reaches start
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
#Setting Path
new_node.path_x = px[:]
new_node.path_y = py[:]
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
continue
###############################################################################################################
###############################################################################################################
#FINDING NEAREST INDICES
nnode = len(self.node_list) + 1
#The circle in which to check parent node and rewiring
r = self.circle * math.sqrt((math.log(nnode) / nnode))
dist_list = [
(node.x - new_point[0])**2 + (node.y - new_point[1])**2
for node in self.node_list
]
#Getting all the indexes within r units of new_node
nearest_indexes = [
dist_list.index(i) for i in dist_list if i <= r**2
]
###############################################################################################################
###############################################################################################################
#GETTING THE PARENT NODE FROM NEAREST INDICES FOR BEST PARENT WITH LEAST COST
costs = [
] # List of Total costs from the start to new_node when attached to parent node in node_list
for index in nearest_indexes:
near_node = self.node_list[index]
point_list = [(near_node.x, near_node.y),
(new_point[0], new_point[1])]
if not check_intersection_scan(point_list, line_obstacles):
costs.append(near_node.cost +
math.sqrt((near_node.x - new_point[0])**2 +
(near_node.y - new_point[1])**2))
else:
costs.append(float("inf"))
# If costs is empty continue
try:
min_cost = min(costs)
except:
continue
# Calculating the minimum cost and selecting the node for which it occurs as parent child
if min_cost == float("inf"):
continue
# Setting the new node as the one with min cost
min_ind = nearest_indexes[costs.index(min_cost)]
new_node = Node(new_point[0], new_point[1])
new_node.parent = self.node_list[min_ind]
new_node.cost = min_cost
###############################################################################################################
###############################################################################################################
#REWIRING
if new_node:
#First append the node to nodelist
self.node_list.append(new_node)
#Rewiring
for ind in nearest_indexes:
#Check for Every Nearest Node in node_list the possibility of rewiring to new node
node_check = self.node_list[ind]
point_list = [(new_node.x, new_node.y),
(node_check.x, node_check.y)]
#Check if the straight line from new_node to node_check is collision free, all others will automatically be collision free
no_coll = not check_intersection_scan(
point_list, line_obstacles)
#Check for Cost improvement
cost_improv = new_node.cost + math.sqrt(
(new_node.x - node_check.x)**2 +
(new_node.y - node_check.y)**2) < node_check.cost
#If both the above conditions are met, set the parent node of node check to new node
if no_coll and cost_improv:
node_check.parent = new_node
###############################################################################################################
###############################################################################################################
#SETTING PATH THE NODE
present_node = new_node
px = []
py = []
while present_node.parent != None:
px.append(present_node.x)
py.append(present_node.y)
present_node = present_node.parent
px.append(self.start.x)
py.append(self.start.y)
new_node.path_x = px[:]
new_node.path_y = py[:]
###############################################################################################################
if animation and iter % 5 == 0:
self.draw_graph(scan, new_node)
###############################################################################################################
#TO PREEMPT BEFORE REACHING MAX ITERATIONS, ONCE GOAL FOUND
if (not self.search_until_max_iter
) and new_node: # check reaching the goal
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
###############################################################################################################
###############################################################################################################
last_index = self.search_best_goal_node(scan)
if last_index:
path = [[self.goal.x, self.goal.y]]
node = self.node_list[last_index]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
return None
print('\n ' + '-' * 30 + "\n> Starting operation ...\n " + '-' * 30 + '\n')
start_time = time.time()
path, node_list = my_tree((0, 0), (-2.5, 2.5), scan)
print("Path planned.")
print('\n ' + '-' * 30 +
"\n> Time taken: {:.4} seconds.\n ".format(time.time() -
start_time) + '-' * 30 +
'\n')
# Visualize tree
# RRT.visualize_tree(node_list, obstacle_list)
visualize_scan(path, scan)
# Testing los path optimizer
line_obs, pts = make_obstacles_scan(scan)
print('\n ' + '-' * 30 + "\n> Starting operation ...\n " + '-' * 30 + '\n')
start_time = time.time()
optimized_path = path_optimizer(path, line_obs)
print("Path optimized.")
print('\n ' + '-' * 30 +
"\n> Time taken: {:.4} seconds.\n ".format(time.time() -
start_time) + '-' * 30 +
'\n')
# # Visualize path
visualize_scan(optimized_path, scan)