def find_path(q_start, q_goal, graph):
path = []
trial = []
# print('goal',q_goal)
# print('start',q_start)
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
grids = graph.getVertices() # nodes/ vertices
print(len(grids))
# Path
f = 0
g = 0
h = 0
for q in range(0, 1):
h = sqrt((grids[0].q[0] - grids[1].q[0])**2 +
(grids[0].q[1] - grids[1].q[1])**2)
f = g + h
open_set.put(grids[0], Value(f=f, g=g))
while len(open_set) > 0:
parent, val = open_set.pop()
closed_set.add(parent)
new = (RoadmapVertex.getConfiguration(parent))
new1 = new[0]
new2 = new[1]
trial = [new1, new2]
if parent == grids[1]:
break
else:
for child in grids:
sssss = RoadmapVertex.getEdge(parent, child.id)
if sssss is not None:
if child not in closed_set:
g_val = distance(child, parent) + val.g
if child not in open_set or open_set.get(
child).g > g_val:
h_val = distance(parent, grids[1])
# print('The H value is:',h_val)
f_val = g_val + h_val
open_set.put(child, Value(f=f_val, g=g_val))
# print('The trial is:', trial)
path.append(trial)
return path
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
obstacles = scene_obstacles # setting the global obstacle variable
x_limit = q_range[0] # the range of x-positions for the robot
y_limit = q_range[1] # the range of y-positions for the robot
theta_limit = q_range[
2] # the range of orientations for the robot (advanced extension)
robot_width, robot_height = robot_dim[0], robot_dim[
1] # the dimensions of the robot, represented as an oriented bounding box
robot_radius = max(robot_width, robot_height) / 2.
# the roadmap
graph = Roadmap()
# scene1= Scene
# qop = scene1.default_query()
graph.addVertex((14.378, -4.328)) # Adding the start as the first vertex.
graph.addVertex((-44.184, -26.821)) # Adding the goal as the last vertex.
"""
The below for loop is used to create samples (vertices) where the coordinate x will be spaced equally
of length 3 and y will be spaced equally of length 2.5 throughout the entire plot. Both the x and y coordinate
will be having a random noise ranging from -0.5 to 0.5.
All these samples are checked first whether they are on the collision free space or not, if they are then
they get added as vertices.
"""
for i in range(0, 33):
x = -48 + 3 * i + (random.uniform(-0.5, 0.5))
for j1 in range(0, 40):
y = -48 + 2.5 * j1 + (random.uniform(-0.5, 0.5))
coll = collision(x, y)
if coll is True:
graph.addVertex((x, y))
Vertices = graph.getVertices(
) # A Vertices list is created with all the vertices being currently placed.
max_dist = 5 # A maximum distance value is set to connect the vertices between them with a edge.
"""
The for loop below will add edge between the vertices which are spaced at a distance less than the max_dist
variable. Once the edge is added, the interpolate function will check if the edge is passing through the C
obstacle space or not. If it is, then the edge gets removed.
Once the edge gets removed, for each parent we shall create a list of children and to remove the edges among
them to reduce the number of unnecessary edges.
"""
for i in Vertices:
list_vertex = []
for j in Vertices:
dist = distance(i, j)
if dist < max_dist and dist != 0:
graph.addEdge(i, j, dist)
some = interpolate(i, j, 0.5)
if some is False:
graph.removeEdge(i, j)
list_vertex.append(j)
if len(list_vertex) >= 2:
for rem in range(0, len(list_vertex)):
for rem1 in range(0, len(list_vertex)):
if rem != rem1:
ss = RoadmapVertex.getEdge(list_vertex[rem],
list_vertex[rem1].id)
if ss is not None:
graph.removeEdge(list_vertex[rem],
list_vertex[rem1])
# uncomment this to export the roadmap to a file
# graph.saveRoadmap("prm_roadmap.txt")
return graph
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
obstacles = scene_obstacles # setting the global obstacle variable
x_limit = q_range[0] # the range of x-positions for the robot
y_limit = q_range[1] # the range of y-positions for the robot
theta_limit = q_range[
2] # the range of orientations for the robot (advanced extension)
robot_width, robot_height = robot_dim[0], robot_dim[
1] # the dimensions of the robot, represented as an oriented bounding box
robot_radius = max(robot_width, robot_height) / 2.
A = np.zeros((1000, 2))
# the roadmap
graph = Roadmap()
list_app = []
vertices_app = []
scene1 = Scene
# qop = scene1.default_query()
goof = graph.addVertex((14.378, -4.328))
goof1 = graph.addVertex((-44.184, -26.821))
for i in range(0, 33):
# x = int(np.random.uniform(-50,50))
# y = int(np.random.uniform(-50,50))
x = -44 + 3 * i + (random.uniform(-0.5, 1))
for j1 in range(0, 33):
y = -44 + 3 * j1 + (random.uniform(-0.5, 1))
coll = collision(x, y)
if coll is True:
graph.addVertex((x, y))
Vertices = graph.getVertices()
max_dist = 5
for i in Vertices:
list_vertex = []
for j in Vertices:
dist = distance(i, j)
# print('in loop dist', dist)
if dist < max_dist and dist != 0:
edge_check = graph.addEdge(i, j, dist)
some = interpolate(i, j, 0.5)
# print('some',some)
if some is False:
graph.removeEdge(i, j)
list_a_x = i.id
list_a_y = j.id
list_a = (list_a_x, list_a_y)
list_app.append(list_a)
list_b = (list_a_y, list_a_x)
if list_b not in list_app:
graph.removeEdge(i, j)
list_vertex.append(j)
if len(list_vertex) >= 2:
for rem in range(0, len(list_vertex)):
for rem1 in range(0, len(list_vertex)):
if rem != rem1:
ss = RoadmapVertex.getEdge(list_vertex[rem],
list_vertex[rem1].id)
if ss is not None:
graph.removeEdge(list_vertex[rem],
list_vertex[rem1])
#print('build',RoadmapVertex.getConfiguration(goof1))
# uncomment this to export the roadmap to a file
# graph.saveRoadmap("prm_roadmap.txt")
return graph
def find_path(q_start, q_goal, graph):
path = []
trial = []
goal0 = q_goal[0]
goal1 = q_goal[1]
start0 = q_start[0]
start1 = q_start[1]
goal = (goal0, goal1)
start = (start0, start1)
# print('The goal is:', goal)
# print('The start is:', start)
# goof = graph.addVertex(goal)
# goof1 = graph.addVertex(start)
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
goal_coll = collision(goal0, goal1)
start_coll = collision(start0, start1)
if goal_coll and start_coll is True:
aa = graph.addVertex((goal0, goal1))
sas = graph.addVertex((start0, start1))
# print(RoadmapVertex.getConfiguration(sas))
# print(ss)
else:
return None
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
grids = graph.getVertices() # nodes/ vertices
# print(grids)
# print(len(grids))
val_app = []
# Path
f = 0
g = 0
h = 0
for q in range(0, 1):
# h = sqrt((start0 - goal0) ** 2 + (start1 - goal1) ** 2)
h = sqrt((grids[0].q[0] - grids[1].q[0])**2 +
(grids[0].q[1] - grids[1].q[1])**2)
f = g + h
# open_set.put(sas, Value(f=f, g=g))
open_set.put(grids[0], Value(f=f, g=g))
while len(open_set) > 0:
parent, val = open_set.pop()
closed_set.add(parent)
val_app.append(val.f)
# print('f', val_app)
# print('inside',len(open_set))
new = (RoadmapVertex.getConfiguration(parent))
new1 = new[0]
new2 = new[1]
trial = [new1, new2]
# if parent == aa:
if parent == grids[1]:
break
else:
for child in grids:
sssss = RoadmapVertex.getEdge(parent, child.id)
if sssss is not None:
if child not in closed_set:
g_val = distance(child, parent) + val.g
# g_val = distance(child, parent) + val.g
if child not in open_set or open_set.get(
child).g > g_val:
# h_val = distance(parent, aa)
h_val = distance(parent, grids[1])
# print('The g value of child is:',g_val)
f_val = g_val + h_val
# print('The f value of child is:', f_val)
open_set.put(child, Value(f=f_val, g=g_val))
if val.g > g_val:
print('more')
# print('The trial is:', trial)
path.append(trial)
# for alpha in grids:
# pare = RoadmapVertex.getEdge(parent,alpha.id)
# if pare is not None:
# print('pare', RoadmapVertex.getConfiguration(alpha))
# parent = [[[' ' for row in range(len(grids[0]))] for col in range(len(grids))],
# [[' ' for row in range(len(grids[0]))] for col in range(len(grids))],
# [[' ' for row in range(len(grids[0]))] for col in range(len(grids))],
# [[' ' for row in range(len(grids[0]))] for col in range(len(grids))]]
# print(parent)
for i in range(len(val_app)):
for j in range(len(val_app)):
if i != j:
if val_app[i] > val_app[j]:
# np.remove(val_app[i])
print('gg')
return path
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
obstacles = scene_obstacles # setting the global obstacle variable
x_limit = q_range[0] # the range of x-positions for the robot
y_limit = q_range[1] # the range of y-positions for the robot
theta_limit = q_range[
2] # the range of orientations for the robot (advanced extension)
robot_width, robot_height = robot_dim[0], robot_dim[
1] # the dimensions of the robot, represented as an oriented bounding box
robot_radius = max(robot_width, robot_height) / 2.
A = np.zeros((1000, 2))
# the roadmap
graph = Roadmap()
list_app = []
list_app = []
vertices_app = []
goof = graph.addVertex((14.378, -4.328))
goof1 = graph.addVertex((-44.184, -26.821))
#graph.addVertex((14.378,-4.328))
for i in range(0, 33):
# x = int(np.random.uniform(-50,50))
# y = int(np.random.uniform(-50,50))
x = -45 + 3 * i + (random.uniform(-0.5, 0.5))
for j1 in range(0, 33):
y = -45 + 3 * j1 + (random.uniform(-0.5, 0.5))
A[i][0] = x
A[i][1] = y
for j in range(0, 26):
xs = []
ys = []
for point in obstacles[j].points:
count = 0
xs.append(point[0])
ys.append(point[1])
x_min = min(xs) - 2
x_max = max(xs) + 2
y_min = min(ys) - 2
y_max = max(ys) + 2
if x_min <= x <= x_max and y_min <= y <= y_max:
count = 1
break
if count != 1:
graph.addVertex((x, y))
# for qq in range(len(A)):
Vertices = graph.getVertices()
max_dist = 5
for i in Vertices:
list_vertex = []
for j in Vertices:
dist = distance(i, j)
# print('in loop dist', dist)
if dist < max_dist and dist != 0:
edge_check = graph.addEdge(i, j, dist)
some = interpolate(i, j, 0.5)
# print('some',some)
if some is False:
graph.removeEdge(i, j)
list_a_x = i.id
list_a_y = j.id
list_a = (list_a_x, list_a_y)
list_app.append(list_a)
list_b = (list_a_y, list_a_x)
if list_b not in list_app:
graph.removeEdge(i, j)
list_vertex.append(j)
if len(list_vertex) >= 2:
for rem in range(0, len(list_vertex)):
for rem1 in range(0, len(list_vertex)):
if rem != rem1:
ss = RoadmapVertex.getEdge(list_vertex[rem],
list_vertex[rem1].id)
if ss is not None:
graph.removeEdge(list_vertex[rem],
list_vertex[rem1])
# p_neighbour = []
# c_neighbour = []
#
# for x in range(len(Vertices)):
# p_neighbour.clear()
# for z in range(len(Vertices)):
# if not x == z:
# parent = RoadmapVertex.getEdge(Vertices[x], Vertices[z].id)
# if not parent is None:
# p_neighbour.append(Vertices[z])
# c_neighbour.clear()
# for u in range(len(p_neighbour)):
# for h in range(len(Vertices)):
# if not u == h:
# children = RoadmapVertex.getEdge(p_neighbour[u], Vertices[h].id)
# if not children is None:
# c_neighbour.append(Vertices[h])
#
# for r in range(len(p_neighbour)):
# for m in range(len(c_neighbour)):
# if p_neighbour[r] == c_neighbour[m]:
# graph.removeEdge(Vertices[x], Vertices[z])
# break
aa = RoadmapVertex.getEdges(goof1)
#print('build',RoadmapVertex.getConfiguration(goof1))
# uncomment this to export the roadmap to a file
# graph.saveRoadmap("prm_roadmap.txt")
return graph