def get_num_voting_booths(geography_list):
voting_booths = []
count = 0
# create polygon
for geography in geography_list:
if type(geography.geometry) is MultiPolygon:
exteriors = [Polygon(polygon.exterior) for polygon in geography.geometry]
exterior_polygon = MultiPolygon(exteriors)
else:
exterior_polygon = Polygon(geography.geometry.exterior)
# List of each of the columns in the public school csv file
school_col_list = ["X", "Y", "OBJECTID", "NCESSCH", "NAME", "OPSTFIPS", "LSTREE", "LCITY", "LSTATE", "LZIP",
"LZIP4",
"STFIP15", "CNTY15", "NMCNTY15", "LOCALE15", "LAT1516", "LON1516", "CBSA15", "NMCBSA15",
"CBSATYPE15", "CSA15", "NMCSA15", "NECTA15", "NMNECTA15", "CD15", "SLDL15", "SLDU15"]
# read each line in the csv file
# each column of the col_list will be filled with corresponding data
school_file = pd.read_csv("../Polling Locations CSV files/Public_School_Locations_201516.csv",
usecols=school_col_list)
# List of each of the columns in the public library csv file
lib_col_list = ["Location Number", "Location Name", "Location Type", "Address", "City", "State", "Zip Code",
"Phone Number",
"County", "Latitude", "Longitude", "Accuracy"]
# read each line in the csv file
# each column of the col_list will be filled with corresponding data
lib_file = pd.read_csv("../Polling Locations CSV files/public_libraries.csv",
usecols=lib_col_list)
# initialize list
voting_booths.append(0)
# Create shapely point based on public school coordinates and check to see if point is contained in polygon
# increment the number of voting booths by the number of points that lie in the polygon
for i in range(len(school_file)):
point = Point(((school_file["X"]).iloc[i]), ((school_file["Y"]).iloc[i]))
if exterior_polygon.contains(point):
voting_booths[count] = voting_booths[count] + 1
for i in range(len(lib_file)):
point = Point(((lib_file["Longitude"]).iloc[i]), ((lib_file["Latitude"]).iloc[i]))
if exterior_polygon.contains(point):
voting_booths[count] = voting_booths[count] + 1
count = count + 1
return voting_booths
def postJuntarPuntos(caminos, centros, separateTol=25):
canchList = []
for cam in caminos:
canchList.append(cam)
MPcanch = MultiPolygon(canchList)
trueCentros = []
while centros:
auxX = [centros[0].x]
auxY = [centros[0].y]
minX = centros[0]
maxX = centros[0]
minY = centros[0]
maxY = centros[0]
centros.pop(0)
selectedPoints = []
for j in range(len(centros)):
if minX.distance(centros[j]) < separateTol or maxX.distance(
centros[j]) < separateTol or minY.distance(
centros[j]) < separateTol or minY.distance(
centros[j]) < separateTol:
lin1 = LineString([centros[j], minX])
lin2 = LineString([centros[j], maxX])
lin3 = LineString([centros[j], minY])
lin4 = LineString([centros[j], maxY])
if not MPcanch.contains(lin1) or not MPcanch.contains(
lin2) or not MPcanch.contains(
lin3) or not MPcanch.contains(lin4):
break
if minX.x > centros[j].x:
minX = centros[j]
if maxX.x < centros[j].x:
maxX = centros[j]
if minY.y > centros[j].y:
minY = centros[j]
if maxY.y < centros[j].y:
maxY = centros[j]
auxX.append(centros[j].x)
auxY.append(centros[j].y)
selectedPoints.append(j)
meanX = stats.mean(list(auxX))
meanY = stats.mean(list(auxY))
trueCentros.append(Point(meanX, meanY))
popCount = 0
for j in selectedPoints:
centros.pop(j - popCount)
popCount += 1
return trueCentros
def group_contains_NE(NE,
group_loc_names,
GN_data,
OSM_data,
PR_shp_lookup,
verbose=False):
"""
Check if group municipalities contain the NE mentioned.
:param NE: NE to check
:param group_loc_names: group municipality names
:param GN_data: GeoNames data
:param OSM_data: OSM data
:param PR_shp_lookup: shape files for all municipalities
:returns contains_NE:: if group contains NE
"""
# get combined shape for group locations
group_shp = MultiPolygon([PR_shp_lookup[x] for x in group_loc_names])
# disambiguate NE to valid location using GN/OSM data
NE_coords = [[-300, -300]] # by default, shape cannot contain coordinate
if (NE in GN_data.keys()):
NE_coords = GN_data[NE]
elif (NE in OSM_data.keys()):
NE_coords = OSM_data[NE]
# containment: if any of the coordinates are contained in group
if (verbose):
logging.debug('NE=%s, group shape %s' % (NE, group_shp))
logging.debug('NE=%s, NE_coords=%s' % (NE, str(NE_coords)))
contains_NE = any(
[group_shp.contains(Point(NE_coord)) for NE_coord in NE_coords])
return contains_NE
def glyph_to_mp(g: fontforge.glyph) -> MultiPolygon:
"""
Converts fontforge glyphs to shapely polygons.
Parameters
----------
g: fontforge glyph
Glyph to be converted
Returns
-------
mp: shapely MultiPolygon
MultyPolygon representing the whole glyph
"""
fg = g.layers[1]
polys = MultiPolygon([Polygon([(p.x, p.y) for p in c]) for c in fg])
mp = MultiPolygon([polys[0]])
if len(polys) > 1:
for h in polys[1:]:
if mp.contains(h):
mp = mp.difference(h)
else:
mp = MultiPolygon([*mp, h])
if type(mp) == Polygon:
mp = MultiPolygon([mp])
return (mp)
def sampleRandomLatLong(wardIndex):
if presampledflag:
i = random.randint(0, len(presampledpoints[wardIndex]) - 1)
(lat, lon) = presampledpoints[wardIndex].loc[i]
return (round(lat, 4), round(lon, 4))
else:
(lon1, lat1, lon2, lat2) = geoDF['wardBounds'][wardIndex]
wardPoly = MultiPolygon(geoDF['geometry'][wardIndex])
while True:
lat = round(random.uniform(lat1, lat2), 4)
lon = round(random.uniform(lon1, lon2), 4)
point = Point(
lon,
lat) #IMPORTANT: Point takes in order of longitude, latitude
if wardPoly.contains(point):
return (lat, lon)
class OSM_Boundary(object):
def __init__(self, relation_element, way_elements, node_elements):
self.polygons = []
self.ways = {}
self.relation = relation_element
self.open = True
reporting = Reporting()
self.id = relation_element.attrib["id"]
self.version = relation_element.attrib["version"]
self.changeset = relation_element.attrib["changeset"]
self.tags = {}
nodes = {}
for element in node_elements:
node = OSM_Node(element)
nodes[node.id] = node
ways = {}
for element in way_elements:
way = OSM_Way(element)
way.add_nodes(nodes)
ways[way.id] = way
for sub in relation_element:
if sub.tag == 'tag':
key = sub.attrib["k"]
value = sub.attrib["v"]
self.tags[key] = value
elif sub.tag == 'member' and sub.attrib["type"] == 'way':
role = sub.attrib.get("role", "")
if role:
raise NotImplementedError, "Role %r for relation way members not supported" % (role,)
way_id = sub.attrib["ref"]
way = ways.get(way_id)
if way:
if not way.complete:
raise ValueError, "Incomplete way: %r" % (way,)
self.ways[way_id] = way
else:
raise ValueError, "Way not found: %r" % (way_id,)
self.name = self.tags.get("name")
if not self.ways:
raise ValueError, "No ways"
self.open = False
ways_left = self.ways.values()
while ways_left:
segment_start = ways_left.pop(0)
polygon = []
for node in segment_start.nodes:
polygon.append((node.lat, node.lon))
last_end = segment_start.end_node
while ways_left:
if last_end is segment_start.start_node:
# cycle ended
break
next = None
for way in ways_left:
if way.start_node is last_end:
last_end = way.end_node
for node in way.nodes[1:]:
polygon.append((node.lat, node.lon))
next = way
break
elif way.end_node is last_end:
last_end = way.start_node
rnodes = list(way.nodes[1:])
rnodes.reverse()
for node in rnodes:
polygon.append((node.lat, node.lon))
next = way
break
if next:
ways_left.remove(next)
else:
# open segment ends
self.open = True
break
self.polygons.append(polygon)
if HAVE_SHAPELY:
reporting.output_msg("info",
"Using Shapely for 'point in polygon' checks")
self.multi_polygon = MultiPolygon([(p, ()) for p in self.polygons])
self. _contains_impl = self._contains_shapely_impl
else:
reporting.output_msg("info",
"Using Python function for the 'point in polygon' checks")
self. _contains_impl = self._contains_python_impl
def __repr__(self):
if self.open:
open_s = "open"
else:
open_s = "closed"
return "<OSM_Boundary #%s %r %s %i ways %i polygons>" % (self.id,
self.name, open_s, len(self.ways), len(self.polygons))
def _contains_python_impl(self, location):
lat = location.lat
lon = location.lon
for pol in self.polygons:
contains = False
plen = len(pol)
i = 0
j = plen - 1
while i < plen:
if ( ((pol[i][0] > lat) != (pol[j][0] > lat)) and
(lon < ((pol[j][1] - pol[i][1]) * (lat - pol[i][0]) / (pol[j][0] - pol[i][0]) + pol[i][1])) ):
contains = not contains
j = i
i += 1
if contains:
return True
return False
def _contains_shapely_impl(self, location):
point = Point(location.lat, location.lon)
return self.multi_polygon.contains(point)
def __contains__(self, location):
if self.open:
return True
return self._contains_impl(location)
randompt = (random.uniform(0, 100), random.uniform(0, 100),
random.uniform(0.0, 2 * math.pi))
#Finds nearest point to the random point
x_y = nearest(pts, randompt)
# euler = DiffDrive_Euler.Euler(x_y,ur,ul)
runge_kutta = DiffDrive_RungeKutta.RungeKutta(x_y, ul, ur)
#this function call returns three possible points straight, left, right
# possible_euler_pts = euler.euler_method()
possible_pts = runge_kutta.runge_kutta()
# print "straight , left , Right ",possible_euler_pts
# finds the nearest of the three points to the random point
newpt = nearest(possible_pts, randompt)
#Collision check whether the newpoint generated lies inside any obstacle or not
if polygons.contains(Point((newpt[0], newpt[1]))):
continue
#Check for new point not crossing the boundary of environment
if newpt[0] > 100 or newpt[0] < 0 or newpt[1] < 0 or newpt[1] > 100:
continue
#generate a line from the nearest point "x-y" to the "newpt" and check whether the line is crossing any obstacle if not add line to the environment
line = [(x_y[0], x_y[1]), (newpt[0], newpt[1])]
if polygons.crosses(LineString(line)):
continue
lines.append(line)
# color.append((0, 1, 1, 1))
# Add the line generated as an edge to the graph from vertex at point "x_y" to "newpt"
graph.addEdge(index1, x_y, index2, newpt)
index1 += 2
index2 += 2
if not utm_zone_number:
utm_zone_number = pos[2]
graph.add_node(item.id, lat=item.lat, lon=item.lon, pos=pos[:2])
#nodes += 1
items += 1
print('{0} items processed'.format(items), end='\r')
print('{0} items processed'.format(items))
if shape_file:
n = graph.number_of_nodes()
i = 0
print('Apply shapefile')
for node in graph.nodes():
p = Point(graph.node[node]['lon'], graph.node[node]['lat'])
if not shape_file.contains(p):
graph.remove_node(node)
i += 1
print('{0}/{1} nodes processed'.format(i, n), end='\r')
print('{0}/{1} nodes processed'.format(i, n))
print('Search for orphaned nodes')
orphaned = set()
n = graph.number_of_nodes()
i = 0
for node in graph.nodes_iter():
if graph.degree(node) == 0:
orphaned.add(node)
i += 1
print('{0}/{1} nodes processed'.format(i, n), end='\r')
class Environment:
def __init__(self, input_file, factor=1):
self.plot_obstacles_polygon = []
self.obs_list = []
self.obs_polygon = MultiPolygon() # Shapely object to store all polygons
self.initial_state, self.goal_state = [], []
self.resolution = 0 # Dimension of the plane.
self.read_env_from_file(input_file)
def read_env_from_file(self, input_file):
# Read json input
try:
print(input_file)
with open(input_file, mode='r', encoding='utf-8') as a_file:
environment = json.loads(a_file.read())
except FileNotFoundError as fl:
print("File not found for JSON ", fl)
exit(1)
except ValueError:
print("Invalid JSON")
exit(1)
except Exception:
print("Unable to process input file")
exit(1)
try:
# Making sure the required entities are defined in the input json file.
environment['resolution'] and environment['obstacles']
environment['initial_state'] and environment['goal_state']
except KeyError:
print("Invalid Environment definition")
exit(1)
self.initial_state, self.goal_state = environment['initial_state'], environment['goal_state']
self.resolution = environment['resolution']
temp_polygon_list = []
for obs in environment['obstacles']:
if not obs.get('shape') and obs.get('property') and obs['property'].get('vertices'):
print("Shape element not present for the obstacles")
continue
if obs['shape'] == 'polygon':
# print("Polygon with vertices %s" %(np.array(obs['property']['vertices'])/100))
polygon = mPolygon(np.array(obs['property']['vertices']))
temp_polygon_list.append(Polygon(obs['property']['vertices']))
self.plot_obstacles_polygon.append(polygon)
self.obs_list.append(obs['property']['vertices'])
else:
print("Undefined shape")
break
self.obs_polygon = MultiPolygon(temp_polygon_list)
def is_point_inside(self, xy):
"""
:param xy: tuple with x coordinate as first element and y coordinate as second element
:return: True if the point is inside the obstacles and False if it isn't
"""
return Point(xy[0], xy[1]).within(self.obs_polygon)
def is_line_inside(self, xy_start, xy_end):
# xy_start is tuple of (x, y) coordinate of one end of the line.
# xy_end is tuple of (x, y) coordinate of the other end of line.
line = LineString([xy_start, xy_end])
return self.obs_polygon.contains(line) or self.obs_polygon.touches(line) or self.obs_polygon.crosses(line)
def draw_env(self, path, key_xy, k_value):
# Method to draw an arrow in the environment
# path is a list of state objects. index 0 maps from-state and index 1 maps to-state.
fig, ax = plt.subplots()
x_path, y_path = [], []
for ls in path:
x_path.append(key_xy(ls)[0])
y_path.append(key_xy(ls)[1])
colors = 100*np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
plt.arrow(x_path[0], y_path[0], x_path[1]-x_path[0], y_path[1]-y_path[0], fc="k", ec="k", head_width=1.55, head_length=1.1)
plt.title("figure" + str(k_value)+".png")
fig.savefig("figure" + str(k_value)+".png", format='png', dpi=fig.dpi)
def animate_path(self, path, key_xy):
fig, ax = plt.subplots()
colors = 100*np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
x_0, y_0 = key_xy(path[0])[0], key_xy(path[0])[1]
x_1, y_1 = key_xy(path[0 + 1])[0], key_xy(path[0 + 1])[1]
dx, dy = x_1 - x_0, y_0 - y_1
qv = ax.quiver(x_0, y_0, dx, dy, angles='xy',scale_units='xy',scale=1)
def animate(i):
x_init, y_init =key_xy(path[i])[0], key_xy(path[i])[1]
x_f, y_f = key_xy(path[i + 1])[0], key_xy(path[i + 1])[1]
dx, dy = x_f - x_init, y_f - y_init
qv.set_UVC(np.array(dx), np.array(dy))
qv.set_offsets((x_init, y_init))
return qv
anim = animation.FuncAnimation(fig, animate, frames=range(0, len(path)-1), interval=500)
plt.show()
def get_apprx_visible_vertices(self, xy_robot):
# To get visible vertices from robot point
# xy_robot should be a tuple of (x, y) coordinate
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, visible_lines = [], []
for obj in pool:
for vertex in obj:
vertex = tuple(vertex)
if vertex == xy_robot:
continue
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
visible_lines.append(line)
visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in visible_lines)
return visible_vertices
def get_actual_visible_vertices(self, xy_robot):
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, line_robot_vertices = [], {}
def line_slope(xy1, xy2):
return (xy2[1] - xy1[1])/(xy2[0] - xy1[0]) if (xy2[0] - xy1[0]) != 0 else sys.maxsize
for obj in pool:
for vertex in obj:
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
if line_slope(xy_robot, vertex) in line_robot_vertices:
if line.length < line_robot_vertices[line_slope(xy_robot, vertex)].length:
line_robot_vertices[line_slope(xy_robot, vertex)] = line
else:
line_robot_vertices[line_slope(xy_robot, vertex)] = line
visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in line_robot_vertices.values())
return visible_vertices
def visibility_line(self, xy_start, xy_end):
# Helper Method to check if the line is intersected by any obstacles.
line = LineString([xy_start, xy_end])
return self.obs_polygon.crosses(line) or self.obs_polygon.contains(line), line
def __str__(self):
return "Obstacle list: %s\nInitial State: %s\nGoal State: %s\nResolution: %d\n" \
% ([cord.xy for cord in self.plot_obstacles_polygon], self.initial_state, self.goal_state, self.resolution)
#Randomly generates a point with (x,y,theta) random values
randompt = (random.uniform(0,100),random.uniform(0,100),random.uniform(0.0,2*math.pi))
#Finds nearest point to the random point
x_y = nearest(pts,randompt)
# euler = DiffDrive_Euler.Euler(x_y,ur,ul)
runge_kutta = DiffDrive_RungeKutta.RungeKutta(x_y,ul,ur)
#this function call returns three possible points straight, left, right
# possible_euler_pts = euler.euler_method()
possible_pts = runge_kutta.runge_kutta()
# print "straight , left , Right ",possible_euler_pts
# finds the nearest of the three points to the random point
newpt = nearest(possible_pts,randompt)
#Collision check whether the newpoint generated lies inside any obstacle or not
if polygons.contains(Point((newpt[0],newpt[1]))):
continue
#Check for new point not crossing the boundary of environment
if newpt[0]>100 or newpt[0]<0 or newpt[1]<0 or newpt[1]>100:
continue
#generate a line from the nearest point "x-y" to the "newpt" and check whether the line is crossing any obstacle if not add line to the environment
line = [(x_y[0],x_y[1]),(newpt[0],newpt[1])]
if polygons.crosses(LineString(line)):
continue
lines.append(line)
# color.append((0, 1, 1, 1))
# Add the line generated as an edge to the graph from vertex at point "x_y" to "newpt"
graph.addEdge(index1,x_y,index2,newpt)
index1 += 2
index2 += 2
print 'Mask_C_FFC'
tic = time.time()
df['Mask_C_FFC'] = [haversine(FFC.Long.map(float)*np.pi/180.0,FFC.Lat.map(float)*np.pi/180.0,float(df.Long[i])*np.pi/180.0,float(df.Lat[i])*np.pi/180.0).min()<150
for i in range(len(df.Lat))]
toc = time.time()
print 'Mask_C_FFC', toc-tic
print 'Mask_SPA_Out'
tic = time.time()
df['Mask_SPA_Out'] = [haversine((-168.9)*np.pi/180.0,-55.0*np.pi/180.0,df.Long[i]*np.pi/180.0,df.Lat[i]*np.pi/180.0)>
1.238*970.0 for i in range(len(df.Lat))]
toc = time.time()
print 'Mask_SPA_Out', toc-tic
print 'Mask_Highland'
tic = time.time()
df['Mask_Highlands'] = [not Maria.contains(Point(df.Long[i],df.Lat[i])) for i in range(len(df.Lat))]
toc = time.time()
print 'Mask_Highland', toc-tic
print 'index'
tic = time.time()
df['Index'] = range(len(df))
toc = time.time()
print 'index', toc-tic
data['feat_df'] = {k: np.array(v.values())[:,np.newaxis] for k,v in df.to_dict().iteritems()}
with open(Output+'LOLA'+str(pix)+'_GRAIL_Dataset_2', 'wb') as fi:
pickle.dump(data, fi, pickle.HIGHEST_PROTOCOL)
class Environment:
def __init__(self, input_file, factor=1):
self.plot_obstacles_polygon = []
self.obs_list = []
self.obs_polygon = MultiPolygon(
) # Shapely object to store all polygons
self.initial_state, self.goal_state = [], []
self.resolution = 0 # Dimension of the plane.
self.read_env_from_file(input_file)
def read_env_from_file(self, input_file):
# Read json input
try:
print(input_file)
with open(input_file, mode='r', encoding='utf-8') as a_file:
environment = json.loads(a_file.read())
except FileNotFoundError as fl:
print("File not found for JSON ", fl)
exit(1)
except ValueError:
print("Invalid JSON")
exit(1)
except Exception:
print("Unable to process input file")
exit(1)
try:
# Making sure the required entities are defined in the input json file.
environment['resolution'] and environment['obstacles']
environment['initial_state'] and environment['goal_state']
except KeyError:
print("Invalid Environment definition")
exit(1)
self.initial_state, self.goal_state = environment[
'initial_state'], environment['goal_state']
self.resolution = environment['resolution']
temp_polygon_list = []
for obs in environment['obstacles']:
if not obs.get('shape') and obs.get(
'property') and obs['property'].get('vertices'):
print("Shape element not present for the obstacles")
continue
if obs['shape'] == 'polygon':
# print("Polygon with vertices %s" %(np.array(obs['property']['vertices'])/100))
polygon = mPolygon(np.array(obs['property']['vertices']))
temp_polygon_list.append(Polygon(obs['property']['vertices']))
self.plot_obstacles_polygon.append(polygon)
self.obs_list.append(obs['property']['vertices'])
else:
print("Undefined shape")
break
self.obs_polygon = MultiPolygon(temp_polygon_list)
def is_point_inside(self, xy):
"""
:param xy: tuple with x coordinate as first element and y coordinate as second element
:return: True if the point is inside the obstacles and False if it isn't
"""
return Point(xy[0], xy[1]).within(self.obs_polygon)
def is_line_inside(self, xy_start, xy_end):
# xy_start is tuple of (x, y) coordinate of one end of the line.
# xy_end is tuple of (x, y) coordinate of the other end of line.
line = LineString([xy_start, xy_end])
return self.obs_polygon.contains(line) or self.obs_polygon.touches(
line) or self.obs_polygon.crosses(line)
def draw_env(self, path, key_xy, k_value):
# Method to draw an arrow in the environment
# path is a list of state objects. index 0 maps from-state and index 1 maps to-state.
fig, ax = plt.subplots()
x_path, y_path = [], []
for ls in path:
x_path.append(key_xy(ls)[0])
y_path.append(key_xy(ls)[1])
colors = 100 * np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon,
cmap=matplotlib.cm.jet,
alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs',
self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
plt.arrow(x_path[0],
y_path[0],
x_path[1] - x_path[0],
y_path[1] - y_path[0],
fc="k",
ec="k",
head_width=1.55,
head_length=1.1)
plt.title("figure" + str(k_value) + ".png")
fig.savefig("figure" + str(k_value) + ".png",
format='png',
dpi=fig.dpi)
def animate_path(self, path, key_xy):
fig, ax = plt.subplots()
colors = 100 * np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon,
cmap=matplotlib.cm.jet,
alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs',
self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
x_0, y_0 = key_xy(path[0])[0], key_xy(path[0])[1]
x_1, y_1 = key_xy(path[0 + 1])[0], key_xy(path[0 + 1])[1]
dx, dy = x_1 - x_0, y_0 - y_1
qv = ax.quiver(x_0,
y_0,
dx,
dy,
angles='xy',
scale_units='xy',
scale=1)
def animate(i):
x_init, y_init = key_xy(path[i])[0], key_xy(path[i])[1]
x_f, y_f = key_xy(path[i + 1])[0], key_xy(path[i + 1])[1]
dx, dy = x_f - x_init, y_f - y_init
qv.set_UVC(np.array(dx), np.array(dy))
qv.set_offsets((x_init, y_init))
return qv
anim = animation.FuncAnimation(fig,
animate,
frames=range(0,
len(path) - 1),
interval=500)
plt.show()
def get_apprx_visible_vertices(self, xy_robot):
# To get visible vertices from robot point
# xy_robot should be a tuple of (x, y) coordinate
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, visible_lines = [], []
for obj in pool:
for vertex in obj:
vertex = tuple(vertex)
if vertex == xy_robot:
continue
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
visible_lines.append(line)
visible_vertices.extend([x.xy[0][1], x.xy[1][1]]
for x in visible_lines)
return visible_vertices
def get_actual_visible_vertices(self, xy_robot):
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, line_robot_vertices = [], {}
def line_slope(xy1, xy2):
return (xy2[1] - xy1[1]) / (xy2[0] - xy1[0]) if (
xy2[0] - xy1[0]) != 0 else sys.maxsize
for obj in pool:
for vertex in obj:
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
if line_slope(xy_robot, vertex) in line_robot_vertices:
if line.length < line_robot_vertices[line_slope(
xy_robot, vertex)].length:
line_robot_vertices[line_slope(xy_robot,
vertex)] = line
else:
line_robot_vertices[line_slope(xy_robot,
vertex)] = line
visible_vertices.extend([x.xy[0][1], x.xy[1][1]]
for x in line_robot_vertices.values())
return visible_vertices
def visibility_line(self, xy_start, xy_end):
# Helper Method to check if the line is intersected by any obstacles.
line = LineString([xy_start, xy_end])
return self.obs_polygon.crosses(line) or self.obs_polygon.contains(
line), line
def __str__(self):
return "Obstacle list: %s\nInitial State: %s\nGoal State: %s\nResolution: %d\n" \
% ([cord.xy for cord in self.plot_obstacles_polygon], self.initial_state, self.goal_state, self.resolution)
