# DEALINGS IN THE SOFTWARE.
#
# PolygonPRM.py
# Written Ian Rankin - October 2020
#
# This example 2D PRM with obstcales and a non-rectangular environment
# A* algorithm finding the shortest point between two points on the PRM.
import rdml_graph as gr
import numpy as np
import matplotlib.pyplot as plt
import shapely.geometry as geo
bounding = geo.Polygon([(-10,25), (0, -35), (10, 25)])
obs = [geo.Polygon([(-5,5), (0, -5), (10, 2)])]
map = {'bounding': bounding, 'obs': obs}
G = gr.PRM( map=map, \
num_points=200,\
r= 10.0, \
sampleF=gr.sample2DPolygon, \
collision=gr.polygonCollision)
path, cost = gr.AStar(G[0], goal = G[50])
# plot the geometric 2d graph
gr.plot2DGeoGraph(G, 'green')
gr.plot2DGeoPath(path, 'red')
plt.show()
n.addEdge(gr.HEdge(n, n2, sign, features=features))
n2.addEdge(gr.HEdge(n2, n, sign, features=features))
n.addEdge(gr.HEdge(n, n3, sign, features=features))
n3.addEdge(gr.HEdge(n3, n, sign, features=features))
n2.addEdge(gr.HEdge(n2, n4, sign, features=features))
n4.addEdge(gr.HEdge(n4, n2, sign, features=features))
n1.addEdge(gr.HEdge(n1, n5, sign, features=features))
n5.addEdge(gr.HEdge(n5, n1, sign, features=features))
n3.addEdge(gr.HEdge(n3, n5, sign, features=features))
n5.addEdge(gr.HEdge(n5, n3, sign, features=features))
n.addEdge(gr.HEdge(n, n5, sign, features=features))
n5.addEdge(gr.HEdge(n5, n, sign, features=features))
gr.plot2DGeoGraph(G, 'blue')
plt.title('Original')
plt.show(block=False)
pickle.dump(G, open("sample.g", "wb"))
loaded = pickle.load(open("sample.g", "rb"))
plt.figure()
gr.plot2DGeoGraph(loaded, 'red')
plt.title('Loaded graph')
plt.show()