def processItem(cluster):
wires = []
_ = cluster.Wires(None, wires)
polys = []
for aWire in wires:
vertices = []
_ = aWire.Vertices(None, vertices)
poly = []
for v in vertices:
p = vg.Point(round(v.X(), 4), round(v.Y(), 4), 0)
poly.append(p)
polys.append(poly)
g = vg.VisGraph()
g.build(polys)
tpEdges = []
vgEdges = g.visgraph.get_edges()
for vgEdge in vgEdges:
sv = topologic.Vertex.ByCoordinates(vgEdge.p1.x, vgEdge.p1.y, 0)
ev = topologic.Vertex.ByCoordinates(vgEdge.p2.x, vgEdge.p2.y, 0)
tpEdges.append(topologic.Edge.ByStartVertexEndVertex(sv, ev))
tpVertices = []
vgPoints = g.visgraph.get_points()
for vgPoint in vgPoints:
v = topologic.Vertex.ByCoordinates(vgPoint.x, vgPoint.y, 0)
tpVertices.append(v)
graph = topologic.Graph(tpVertices, tpEdges)
return graph
def main():
graph = vg.VisGraph()
graph.load('ntu_new_2')
# start = map(float, raw_input('Start: ').strip().split())
# end = map(float, raw_input('End: ').strip().split())
while True:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
s.listen(1)
print 'Waiting for connection...'
conn, addr = s.accept()
print 'Connected by: ', addr
while True:
data = conn.recv(1024)
if not data:
break
print 'Data received: ', data
startx, starty, endx, endy = map(float, data.split())
start = vg.Point(startx, starty)
end = vg.Point(endx, endy)
c_start = graph.point_in_polygon(start)
c_end = graph.point_in_polygon(end)
path = []
if c_start != -1:
path.append(str(start.x) + ' ' + str(start.y))
start = graph.closest_point(start, c_start)
if c_end != -1:
end = graph.closest_point(end, c_end)
shortest_path = graph.shortest_path(start, end)
for point in shortest_path:
path.append(str(point.x) + ' ' + str(point.y))
path.append(str(endx) + ' ' +str(endy))
print 'Data sent: ', ' '.join(path)
conn.sendall(' '.join(path))
conn.close()
def setup_method(self, method):
self.g = vg.VisGraph()
self.point_a = Point(0, 0)
self.point_b = Point(4, 0)
self.point_c = Point(2, 4)
self.point_d = Point(1, 0.5)
self.g.build([[self.point_a, self.point_b, self.point_c]])
def ShorestPathBtRg(self, regions):
"""
calculate shoresr path between any two labeled regions
:param regions: regions
:return: dict (region, region) : length
"""
polys = [[vg.Point(0.4, 1.0), vg.Point(0.4, 0.7), vg.Point(0.6, 0.7), vg.Point(0.6, 1.0)],
[vg.Point(0.3, 0.2), vg.Point(0.3, 0.0), vg.Point(0.7, 0.0), vg.Point(0.7, 0.2)]]
g = vg.VisGraph()
g.build(polys, status=False)
min_len_region = dict()
for key1, value1 in regions.items():
for key2, value2 in regions.items():
init = value1[:2]
tg = value2[:2]
# shorest path between init and tg point
shortest = g.shortest_path(vg.Point(init[0], init[1]), vg.Point(tg[0], tg[1]))
# (key2, key1) is already checked
if (key2, key1) in min_len_region.keys():
min_len_region[(key1, key2)] = min_len_region[(key2, key1)]
else:
# different regions
if key1 != key2:
dis = 0
for i in range(len(shortest)-1):
dis = dis + np.linalg.norm(np.subtract((shortest[i].x, shortest[i].y), (shortest[i+1].x, shortest[i+1].y)))
min_len_region[(key1, key2)] = dis
# same region
else:
min_len_region[(key1, key2)] = 0
return min_len_region
def __init__(self, n_robot, acpt, ts, buchi_graph, init, seg, step_size,
no):
"""
:param acpt: accepting state
:param ts: transition system
:param buchi_graph: Buchi graph
:param init: product initial state
"""
self.robot = n_robot
self.acpt = acpt
self.goals = []
self.ts = ts
self.buchi_graph = buchi_graph
self.init = init
self.seg = seg
self.step_size = step_size
self.dim = len(self.ts['workspace'])
uni_ball = [
1, 2, 3.142, 4.189, 4.935, 5.264, 5.168, 4.725, 4.059, 3.299, 2.550
]
# uni_v = uni_ball[self.robot*self.dim]
uni_v = np.power(np.pi, self.robot * self.dim /
2) / math.gamma(self.robot * self.dim / 2 + 1)
self.gamma = np.ceil(
4 * np.power(1 / uni_v, 1. /
(self.dim * self.robot))) # unit workspace
self.tree = DiGraph(type='PBA', init=init)
self.group = dict()
label = []
for i in range(self.robot):
l = self.label(init[0][i])
# exists one sampled point lies within obstacles
if l != '':
l = l + '_' + str(i + 1)
label.append(l)
self.tree.add_node(init, cost=0, label=label)
self.add_group(init)
# probability
self.p = 0.9
# threshold for collision avoidance
self.threshold = 0.02
# polygon obstacle
polys = [[
vg.Point(0.4, 1.0),
vg.Point(0.4, 0.7),
vg.Point(0.6, 0.7),
vg.Point(0.6, 1.0)
],
[
vg.Point(0.3, 0.2),
vg.Point(0.3, 0.0),
vg.Point(0.7, 0.0),
vg.Point(0.7, 0.2)
]]
self.g = vg.VisGraph()
self.g.build(polys, status=False)
# region that has ! preceding it
self.no = no
def test_point_in_polygon():
g = vg.VisGraph()
point_a = Point(0, 0)
point_b = Point(4, 0)
point_c = Point(2, 4)
point_d = Point(1, 0.5)
g.build([[point_a, point_b, point_c]])
assert g.point_in_polygon(point_d) != -1
def test_closest_point_edge_point(self):
"""Test where the cp is a end-point of a polygon edge. Can end up with
cp extending into polygon instead of outside it."""
g = vg.VisGraph()
g.build([[Point(0, 1), Point(2, 0), Point(1, 1), Point(2, 2)]])
p = Point(1, 0.9)
pid = g.point_in_polygon(p)
cp = g.closest_point(p, pid, length=0.001)
assert g.point_in_polygon(cp) == -1
def test_build_2_core(self):
world = vg.VisGraph()
world.build(self.polys, workers=2)
assert len(world.graph.get_points()) == 310
assert len(world.graph.get_edges()) == 310
assert len(world.visgraph.get_edges()) == 1156
s = "Graph points: {} edges: {}, visgraph edges: {}"
print(s.format(len(world.graph.get_points()),
len(world.graph.get_edges()),
len(world.visgraph.get_edges())))
def findPathPoly(self, sourceP, targetP, objList, layoutPoly):
"""
calculating shortest path from sourceP point to targetP that avoid polygon shape obstacles
sourceP/targetP - 2D points
objList - List of obstacle objects (polygons, each is a list of 2D points).
Should Contains the object's polygon and forward facing edge ???
layoutPoly - Nx2 list of ordered vertices defining a 2D polygon of N vertices - room polygon layout
last point NEQ first point
=>>>>>>> Assuming polygons DO NOT intersect
"""
nObj = len(objList)
objListVg = []
# Transform to pyvisgraph format
for n in range(nObj):
tmpPoly = []
for p in objList[n]:
tmpPoly.append(pvg.Point(p.x, p.y))
objListVg.append(tmpPoly)
# Start building the visibility graph
graph = pvg.VisGraph()
refPoint = pvg.Point(sourceP[0].x, sourceP[0].y)
workers = 1
graph.build_mod(objListVg, workers, None,
refPoint) # , workers=workers)
# graph.build(objListVg) #, workers=workers)
# Get the shortest path
shortest_path = []
path_distance = []
direct_distance = []
for n in range(len(sourceP)):
sP = pvg.Point(sourceP[n].x, sourceP[n].y)
tP = pvg.Point(targetP[n].x, targetP[n].y)
spath = graph.shortest_path(sP, tP)
# Calculate the total distance of the shortest path
pdistance = 0
prev_point = spath[0]
for point in spath[1:]:
pdistance += np.sqrt((prev_point.y - point.y)**2 +
(prev_point.x - point.x)**2)
prev_point = point
shortest_path.append(spath)
path_distance.append(pdistance)
dDist = np.sqrt((targetP[n].x - sourceP[n].x)**2 +
(targetP[n].y - sourceP[n].y)**2)
direct_distance.append(dDist)
# print('Shortest path distance: {}'.format(path_distance))
return shortest_path, path_distance, direct_distance
def __init__(self):
self.polygons = []
self.work_polygon = []
self.mouse_point = None
self.start_point = None
self.end_point = None
self.shortest_path = []
self.g = vg.VisGraph()
self.built = False
self.mode_draw = True
self.mode_path = False
def __init__(self, index, points, obstacles):
self.index = index
self.points = points
obstacle_points = []
for o in obstacles:
ob = []
for p in o:
ob.append(vg.Point(p[0], p[1]))
obstacle_points.append(ob)
_g = vg.VisGraph()
_g.build(obstacle_points, workers=cpu_count())
self._g = _g
def test():
m = open('newMap.txt')
polys = []
for line in m:
points = (line.strip()).split(',')
poly = []
for point in points:
point = map(float, point.split())
poly.append(vg.Point(point[0], point[1]))
polys.append(poly)
graph = vg.VisGraph()
graph.build(polys, workers=4)
shortest = graph.shortest_path(vg.Point(-520, -70), vg.Point(450, -1300))
print shortest
def testApp():
segments2 = [] # figures
root = Tk()
root.geometry("1270x650")
c = Canvas(root, bg="black", height=700, width=1400)
Figures = obtainRandomFigures()
drawRandomFigures(c, Figures)
pointsX, pointsY = obtainXListAndYList(Figures)
c.create_oval(x1 - 5, y1 - 5, x1 + 5, y1 + 5, fill="red")
c.create_oval(x2 - 5, y2 - 5, x2 + 5, y2 + 5, fill="green")
drawCircles(pointsX, pointsY, c)
polys = []
for list in Figures:
tmpList = []
for i in range(0, len(list), 2):
x = list[i]
y = list[i + 1]
newPoint = vg.Point(x, y)
tmpList.append(newPoint)
polys.append(tmpList)
g = vg.VisGraph()
g.build(polys)
shortest = g.shortest_path(vg.Point(x1, y1), vg.Point(x2, y2))
cnt = 0
for n in range(len(shortest)):
try:
c.create_line(shortest[n].x,
shortest[n].y,
shortest[n + 1].x,
shortest[n + 1].y,
width=0.9,
fill="green")
except:
pass
#hilo1 = threading.Thread(target=avanzarPunto, args=(shortest,c,))
#hilo1.start()
c.pack()
root.mainloop()
def getPath(movingRobot, sleepingRobot):
polys = []
for ob in obs:
polygon = []
for coords in ob:
polygon.append(vg.Point(*coords))
polys.append(polygon)
g = vg.VisGraph()
g.build(polys)
paths = []
path = shortest = g.shortest_path(movingRobot, sleepingRobot)
# Finds the path between the nodes from the start robot to all the other robots and appends the path to paths
# for robot in sleepingRobots:
# path = shortest = g.shortest_path(movingRobot, robot)
# paths.append(path)
return path
def test_collin5(self):
r = 0.2 # Radius of polygon
n = 4 # Sides of polygon
c = Point(1.0, 1.0) # Center of polygon
verts = []
for i in range(n):
verts.append(
Point(r * cos(2 * pi * i / n - pi / 4) + c.x,
r * sin(2 * pi * i / n - pi / 4) + c.y))
g = vg.VisGraph()
g.build([verts])
s = Point(0, 0)
t = Point(1.7, 1.7)
shortest = g.shortest_path(s, t)
visible = visible_vertices(t, g.graph, s, None)
assert verts[3] not in visible
assert verts[1] not in shortest
assert verts[3] not in shortest
def __init__(self, polys, origin, destination):
n = len(polys)
m = len(polys[0])
self.polys = [[0] * m for i in range(n)]
for i in range(n):
for j in range(m):
self.polys[i][j] = vg.Point(polys[i][j][0], polys[i][j][1])
self.origin = vg.Point(origin[0], origin[1])
self.destination = vg.Point(destination[0], destination[1])
self.g = vg.VisGraph()
self.g.build(self.polys)
self.path = self.construct_path()
self.waypoint = []
for i in range(len(self.path)):
self.waypoint.append([self.path[i].x, self.path[i].y, 0, 0])
super(AStarGraph, self).__init__(self.waypoint)
def computeVisibilityGraph(contoursMapped):
"""Given the dilated obstacles, compute the visibility graph
Parameters
----------
contoursMapped : list of list of list
There are several dilated obstacles
Each one has several extremities
Each extremity has (x, y) coordinates
Returns
-------
g : object of class Graph
the visibility graph of our problem
possibleDisplacement : dictionary
key : tuple containing (x,y) coordinates of one of the edges of the dilated obstacles
value : list of all other edges visible from the key edge
"""
# Compute the visibility graph
polys = [[] for _ in contoursMapped]
for obstacle, i in zip(contoursMapped, range(len(contoursMapped))):
for extremity in obstacle:
polys[i].append(vg.Point(extremity[X], extremity[Y]))
g = vg.VisGraph()
g.build(polys)
# Create a dictionary where each extremety point has several visible points i.e possible destinations
possibleDisplacement = {}
for obstacle, i in zip(contoursMapped, range(len(contoursMapped))):
for extremity, j in zip(obstacle, range(len(obstacle))):
visible = g.find_visible(polys[i][j])
possibleDisplacement[(extremity[X],
extremity[Y])] = [[point.x, point.y]
for point in visible]
visible.clear()
return g, possibleDisplacement
def run(polygons, robots, case_number):
# print("Now Processing #case :" + str(case_number) +
# Robots = " + str(len(robots)) + " #polygons = " + str(len(polygons)))
polygon_objs = []
for polygon in polygons:
poly = []
for p in polygon:
poly.append(vg.Point(p[0], p[1]))
polygon_objs.append(poly)
graph = vg.VisGraph()
# graph.load('case' + str(case_number) + '.pk1')
graph.build(polygon_objs)
path_map = [[None for i in range(len(robots))] for j in range(len(robots))]
for i, s in enumerate(robots):
for j, d in enumerate(robots):
if i != j and j > i:
source, dest = vg.Point(s[0], s[1]), vg.Point(d[0], d[1])
path_map[i][j] = graph.shortest_path(source, dest)
ret_map = []
for i in range(len(robots)):
new_array = []
for j in range(len(robots)):
pts = []
if j == i:
pass
elif j < i:
pts = list(reversed(ret_map[j][i]))
else:
for p in path_map[i][j]:
pts.append([p.x, p.y])
new_array.append(pts)
ret_map.append(new_array)
# return ret_map
robot_paths = make_decisions(ret_map)
return robot_paths
def test_angle2_function():
polys = [[
Point(353.6790486272709, 400.99387840984855),
Point(351.1303807396073, 398.8696192603927),
Point(349.5795890382704, 397.8537806679034),
Point(957.1067811865476, -207.10678118654744),
Point(-457.10678118654766, -207.10678118654744),
Point(-457.10678118654744, 1207.1067811865476),
Point(957.1067811865476, 1207.1067811865473),
Point(353.52994294901674, 606.0798841165788),
Point(354.0988628008279, 604.098862800828),
Point(354.52550331744527, 601.3462324760635),
Point(352.6969055662087, 602.6943209889012),
Point(351.22198101804634, 603.781672670995),
Point(247.0, 500.0),
Point(341.8964635104416, 405.50444716676054),
Point(349.24224903733045, 410.671256247085),
Point(350.84395848060774, 407.17766877398697)
]]
v = vg.VisGraph()
v.build(polys)
def getPathWaypoints(self, startPoint, endPoint):
if startPoint is None or startPoint[0] is None or startPoint[1] is None:
return None
graph = vg.VisGraph()
obstacles = mergePolygons(self.cSpaceObstacles)
finalObs = []
for obs in obstacles:
#print("OBS", obs)
obstacleData = []
for p in obs:
try:
x = p[0]
y = p[1]
obstacleData.append(vg.Point(x, y))
except:
pass
finalObs.append(obstacleData)
self.obstacleCorners = []
for obstacle in finalObs:
for pt in obstacle:
self.obstacleCorners.append([pt.x, pt.y])
self.obstacleCorners = np.array(self.obstacleCorners, dtype=np.float32)
try:
graph.build(finalObs)
self.graph = graph
self.origin = vg.Point(startPoint[0], startPoint[1])
self.dest = vg.Point(endPoint[0], endPoint[1])
path = graph.shortest_path(vg.Point(startPoint[0], startPoint[1]),
vg.Point(endPoint[0], endPoint[1]))
return [(p.x, p.y) for p in path]
except:
return [startPoint]
def graph(polygons, robots):
polys = []
temp = []
shortest = []
z = len(robots) - 1
for x in polygons:
for a, b in x:
# print a, "then", b
temp.append(vg.Point(a, b))
polys.append(temp)
temp = []
# print polys
g = vg.VisGraph()
g.build(polys)
shortest = g.shortest_path((vg.Point(robots[0][0], robots[0][1])),
(vg.Point(robots[1][0], robots[1][1])))
for i in xrange(1, len(robots) - 1, 1):
# print i
# shortest.pop()
shortest += g.shortest_path(
(vg.Point(robots[i][0], robots[i][1])),
(vg.Point(robots[i + 1][0], robots[i + 1][1])))
print shortest
def main():
graph = vg.VisGraph()
graph.load('ntu_new_2')
start = map(float, raw_input('Start: ').strip().split())
end = map(float, raw_input('End: ').strip().split())
shortest_path = graph.shortest_path(vg.Point(start[0], start[1]), vg.Point(end[0], end[1]))
x = [point.x for point in shortest_path]
y = [point.y for point in shortest_path]
pyplot.plot(x, y)
m = open('newMap_2.txt')
for line in m:
points = (line.strip()).split(',')
x = []
y = []
for point in points:
point = map(float, point.split())
x.append(point[0])
y.append(point[1])
x.append(x[0])
y.append(y[0])
pyplot.plot(x, y)
pyplot.show()
def __init__(self, workspace, geodesic, buchi, task, init_state,
init_label, init_angle, segment, para):
"""
initialization of the tree
:param workspace: workspace
:param buchi: buchi automaton
:param init_state: initial location of the robots
:param init_label: label generated by the initial location
:param segment: prefix or suffix part
:param para: parameters regarding biased-sampling method
"""
# parameters regarding workspace
self.workspace_instance = workspace
self.workspace = workspace.workspace
self.dim = len(self.workspace)
# self.regions = workspace.regions
self.obstacles = workspace.obs
self.node_landmark = Landmark()
self.node_landmark.update_from_workspace(workspace)
self.geodesic = geodesic
self.robot = buchi.number_of_robots
# parameters regarding task
self.buchi = buchi
self.task = task
self.accept = self.buchi.buchi_graph.graph['accept']
self.init = init_state
# initlizing the tree
self.biased_tree = DiGraph(type='PBA', init=self.init)
self.biased_tree.add_node(self.init, cost=0, label=init_label, \
angle=init_angle, lm=self.node_landmark, \
node_id=0)
self.node_count = 1
# parameters regarding TL-RRT* algorithm
self.goals = []
self.step_size = para['step_size']
self.segment = segment
self.lite = para['is_lite']
# size of the ball used in function near
uni_v = np.power(np.pi, self.robot * self.dim /
2) / math.gamma(self.robot * self.dim / 2 + 1)
self.gamma = np.ceil(
4 * np.power(1 / uni_v, 1. /
(self.dim * self.robot))) # unit workspace
# parameters regarding biased sampling
# group the nodes in the tree by the buchi state
self.group = dict()
self.add_group(self.init)
# select final buchi states
if self.segment == 'prefix':
self.b_final = self.buchi.buchi_graph.graph['accept'][0]
else:
self.b_final = self.buchi.buchi_graph.graph['accept']
self.min_dis = np.inf
self.q_min2final = []
self.not_q_min2final = []
self.update_min_dis2final_and_partition(self.init)
# probability of selecting q_p_closest
self.p_closest = para['p_closest']
# weight when selecting x_rand
self.y_rand = para['y_rand']
# threshold for collision avoidance
self.threshold = para['threshold']
# Updates landmark covariance when inside sensor range
self.update_covariance = para['update_covariance']
# sensor range in meters
self.sensor_range = para['sensor_range']
# sensor measurement noise
self.sensor_R = para['sensor_R']
# polygon obstacle for visibility-based method
polys = []
for poly in self.obstacles.values():
polys.append([
vg.Point(x[0], x[1]) for x in list(poly.exterior.coords)[:-1]
])
self.g = vg.VisGraph()
self.g.build(polys, status=False)
polygons.append(line)
robotpoints = list(literal_eval(robots[0]))
drawRobots(robotpoints)
polygonpoints = list(literal_eval(polygons[0]))
drawPolygons(polygonpoints)
polys = []
for arraypoly in polygonpoints: #adding poygons to the vg
temp = []
for (x, y) in arraypoly:
temp.append(vg.Point(x, y))
polys.append(temp)
g = vg.VisGraph()
g.build(polys)
paths = []
for (x, y) in robotpoints: #finding shortest path between any two vertices
for (a, b) in robotpoints:
shortest = g.shortest_path(vg.Point(x, y), vg.Point(a, b))
paths.append(shortest)
print paths
for path in paths: #drawing the visibility graph
print path
temppath = []
for coord in path:
def __init__(self, n_robot, acpt, ts, buchi_graph, init, step_size, no):
"""
:param acpt: accepting state
:param ts: transition system
:param buchi_graph: Buchi graph
:param init: product initial state
"""
self.robot = n_robot
self.acpt = acpt
self.goals = []
self.ts = ts
self.buchi_graph = buchi_graph
self.init = init
self.step_size = step_size
self.dim = len(self.ts['workspace'])
uni_v = np.power(np.pi, self.robot * self.dim /
2) / math.gamma(self.robot * self.dim / 2 + 1)
self.gamma = np.ceil(
4 * np.power(1 / uni_v, 1. /
(self.dim * self.robot))) # unit workspace
self.tree = DiGraph(type='PBA', init=init)
self.group = dict()
# label of init
label = []
for i in range(self.robot):
l = self.label(self.init[0][i])
# exists one sampled point lies within obstacles
if l != '':
l = l + '_' + str(i + 1)
label.append(l)
self.tree.add_node(self.init, cost=0, label=label)
# label of acpt
label = []
for i in range(self.robot):
l = self.label(self.init[0][i])
# exists one sampled point lies within obstacles
if l != '':
l = l + '_' + str(i + 1)
label.append(l)
self.tree.add_node(self.acpt, cost=0, label=label)
# index of robot whose location needs to be changed
self.change = self.match(label)
# best node
self.x_min = self.init
self.c = np.linalg.norm(np.subtract(self.init[0], self.acpt[0]))
# probability for selecting a node
self.p = 0.9
# target point
self.weight = 0.5
# threshold for collision avoidance
self.threshold = 0.005
# polygon obstacle
polys = [[
vg.Point(0.4, 1.0),
vg.Point(0.4, 0.7),
vg.Point(0.6, 0.7),
vg.Point(0.6, 1.0)
],
[
vg.Point(0.3, 0.2),
vg.Point(0.3, 0.0),
vg.Point(0.7, 0.0),
vg.Point(0.7, 0.2)
]]
self.g = vg.VisGraph()
self.g.build(polys, status=False)
# region that has ! preceding it
self.no = no
def setup_method(self, method):
self.world = vg.VisGraph()
self.world.load('world.pk1')
self.origin = vg.Point(100, -20)
self.destination = vg.Point(25, 75)
SOFTWARE.
"""
import pyvisgraph as vg
from haversine import haversine
# In this example we will find the shortest path between two points on a
# sphere, i.e. on earth. To calculate the total distance of that path, we
# need to use the great circle formula. We use the haversine package for this.
# Example points
start_point = vg.Point(12.568337, 55.676098) # Copenhagen
end_point = vg.Point(103.851959, 1.290270) # Singapore
# Load the visibility graph file. If you do not have this, please run
# 1_build_graph_from_shapefiles.py first.
graph = vg.VisGraph()
graph.load('GSHHS_c_L1.graph')
# Get the shortest path
shortest_path = graph.shortest_path(start_point, end_point)
# Calculate the total distance of the shortest path in km
path_distance = 0
prev_point = shortest_path[0]
for point in shortest_path[1:]:
# Add miles=True to the end of the haversine call to get result in miles
path_distance += haversine((prev_point.y, prev_point.x),
(point.y, point.x))
prev_point = point
# If you want to total distance in nautical miles:
# path_distance = path_distance*0.539957
def get_visibility_graph(points_list):
graph = vg.VisGraph()
graph.build(points_list, workers=3, status=False)
return graph
print(
'-------------- suffix path for {0}-th pre-goal (of {1} in total) --------------'
.format(i + 1, len(tree_pre.goals)))
print('{0}-th pre-goals: {1} accepting goals found'.format(
i + 1, len(tree_suf.goals)))
# couldn't find the path within predtermined number of iterations
if not cost_path_suf_cand:
del cost_path_pre[i]
print('delete {0}-th item in cost_path_pre, {1} left'.format(
i + 1, len(cost_path_pre)))
continue
# not trivial suffix path
if cost_path_suf_cand[0][1]:
# find the remaining path to close the loop using the visibility graph
tree_suf.g = vg.VisGraph()
tree_suf.g.build(polys_suf, status=False)
for k in range(len(cost_path_suf_cand)):
cost, path_free = path_via_visibility(tree_suf,
cost_path_suf_cand[k][1])
cost_path_suf_cand[k][0] += cost
cost_path_suf_cand[k][1] += path_free[1:]
# order according to cost
cost_path_suf_cand = OrderedDict(
sorted(cost_path_suf_cand.items(), key=lambda x: x[1][0]))
min_cost = list(cost_path_suf_cand.keys())[0]
cost_path_suf = cost_path_suf_cand[min_cost]
# update the best path so far
if para['weight'] * cost_path_pre[i][0] + (1-para['weight']) * cost_path_suf[0] < \
para['weight'] * opt_cost[0] + (1-para['weight']) * opt_cost[1]:
def main():
start_x = start_y = 0
end_x = end_y = 0
#initialize the display
pygame.init()
noOfMouseClick = 0
SURFACE = pygame.display.set_mode((900, 600))
SURFACE.fill((255, 255, 255))
pygame.display.set_caption("Robot Path Planning")
#ask for input from user in the GUI itself
answer = inputbox.ask(SURFACE, "Name of the file: ")
my_list = get_input_file(answer)
polys = []
poly2 = []
SURFACE.fill((255, 255, 255))
while True:
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
sys.exit()
elif event.type == KEYUP:
for i in my_list:
zipped = zip(i[::2], i[1::2])
poly2.append(zipped)
pygame.draw.polygon(SURFACE, (0, 0, 0), zipped, 1)
poly1 = []
for j, k in zipped:
poly1.append(vg.Point(j, k))
polys.append(poly1)
#the first click takes the point as the start point
elif event.type == MOUSEBUTTONDOWN and event.button == 1 and noOfMouseClick == 0:
start_x, start_y = event.pos
pygame.draw.circle(SURFACE, (0,0,0), [start_x, start_y], 5)
robot_char = pygame.Rect(start_x, start_y, 20, 30)
robot = pygame.image.load("Cartoon_Robot.png")
robot = pygame.transform.scale(robot, (50,50))
SURFACE.blit(robot, robot_char)
noOfMouseClick = 1
#the second click is the place where the robot's path ends
elif event.type == MOUSEBUTTONDOWN and event.button == 1 and noOfMouseClick == 1:
end_x, end_y = event.pos
pygame.draw.circle(SURFACE, (0, 0, 0), [end_x, end_y], 5)
noOfMouseClick = 2
#on the third click the visibility graph is drawn
elif event.type == MOUSEBUTTONDOWN and event.button == 1 and noOfMouseClick == 2:
noOfMouseClick = 3
g = vg.VisGraph()
g.build(polys)
gra = build_graph(g, vg.Point(start_x, start_y), vg.Point(end_x, end_y))
listofEdges = list(gra.get_edges())
for i in listofEdges:
pygame.draw.line(SURFACE, (255, 0, 0), [i.p1.x, i.p1.y], [i.p2.x, i.p2.y], 2)
pygame.draw.circle(SURFACE, (0, 0, 0), [start_x, start_y], 5)
pygame.draw.circle(SURFACE, (0, 0, 0), [end_x, end_y], 5)
for i in poly2:
pygame.draw.polygon(SURFACE, (0, 0, 0), i)
#on the fourth click, the shortest path is calculated and drawn
elif event.type == MOUSEBUTTONDOWN and event.button == 1 and noOfMouseClick == 3:
noOfMouseClick = 4
poly2.append([(start_x, start_y)])
poly2.append([(end_x, end_y)])
listofNodes = []
for i in poly2:
for j, k in i:
listofNodes.append(Node(j,k))
shortest_path = aStar(listofNodes, listofEdges, start_x, start_y, end_x, end_y)
for i in listofNodes:
if i.x == end_x and i.y == end_y:
shortest_distance = i.heuristic + i.pathcost
for i, j in shortest_path:
robot_char.x = i
robot_char.y = j
SURFACE.blit(robot, robot_char)
print "Start point is :"
print start_x ,start_y
print "Goal point is :"
print end_x,end_y
print "Shortest path is :"
print shortest_path
print "Shortest Distance is:"
print shortest_distance
for i in range(len(shortest_path) - 1):
pygame.draw.line(SURFACE, (0, 255, 0), shortest_path[i], shortest_path[i+1], 5)
pygame.draw.circle(SURFACE, (0, 0, 0), [start_x, start_y], 5)
pygame.draw.circle(SURFACE, (0, 0, 0), [end_x, end_y], 5)
pygame.display.update()
