vehicle_navigation_shortest_path.py

# -*- coding: utf-8 -*-
"""Vehicle_Navigation-Shortest_Path.ipynb

Automatically generated by Colaboratory.

Original file is located at
    https://colab.research.google.com/drive/103unhaid5NTquk2vVaEhzRrYFdc7e2Y5
"""

!pip install osmnx==0.16.2

!pip install contextily

# Commented out IPython magic to ensure Python compatibility.
#Import necessary libraries.
import numpy as np
import math
import folium
import contextily as ctx
import shapely
import osmnx as ox
import geopandas as gpd
from pyproj import Transformer
from shapely.geometry import  Point, Polygon, LineString
from matplotlib.ticker import FormatStrFormatter
from matplotlib.patches import Circle
from osgeo import ogr
import matplotlib.pyplot as plt
# %matplotlib inline

def pointreproject(point_x,point_y,inp_CRS,out_CRS):
  
  """Converts from (latitude, longitude) to native map projection (x,y) coordinates and vice versa. 

    Args:
        point_x: input CRS coordinate (x or latitude). 
        point_y: input CRS coordinate (y or longitude).
        inp_CRS: reference system of input coordinates: 'epsg:4326','epsg:3857'...
        out_CRS: reference system of output coordinates: 'epsg:4326','epsg:3857'...

    Returns:
        A tuple (latitude, longitude) or (x,y) of the reprojected coordinates of the input point in the output CRS.
          
  """
  transformer = Transformer.from_crs(inp_CRS, out_CRS)
  out_x,out_y = transformer.transform(point_x, point_y)
  transformer = []
  
  return out_x,out_y

def rgraphfrompoints(starting_point,ending_point):

  """Creates a graph, with edges (roads) and nodes (road intersections), based on the two given points.
     The two given points must be in the WGS84 coordinate reference system and in the form of (latitude, longitude).  

    Args:
        starting_point: the route's starting point. -> A tuple of (latitude, longitude).
        ending_point: the route's ending point. -> A tuple of (latitude, longitude).

    Returns:
        A reprojected (from EPSG:4326 to EPSG:3857) MultiDiGraph from the osmnx package.
          
  """
  #Input points geographical coordinates.  
  lat1 = starting_point[0]
  lat2 = ending_point[0]
  long1 = starting_point[1]
  long2 = ending_point[1]
  
  #Find the function's argument values.
  north = lat2
  south = lat1
  if  lat1 > lat2:
    north = lat1 
    south = lat2

  east = long2
  west = long1
  if  long1 > long2:
    east = long1 
    west = long2

  #Construct the road graph.
  roadgraph = ox.graph_from_bbox(north+0.01, south-0.01, east+0.01, west-0.01, network_type = 'drive', simplify=False )

  #Reproject the road graph.
  projected_roadgraph = ox.projection.project_graph(roadgraph, to_crs='epsg:3857')

  return projected_roadgraph

def nearest_edge_node_id(point,roadgraph):

  """Finds the nearest network's node id for the given point, which lies in the nearest network edge.
     Input Point -> Nearest Edge Point -> Nearest Network Node (Route Starting/Ending Point).
     The given point's coordinates must be in the EPSG:4326.  

    Args:
        point: The point:(latitude, longitude) that needs to be transfered to the nearest network node..
        roadgraph: A reprojected (from EPSG:4326 to EPSG:3857) MultiDiGraph from the osmnx package.

    Returns:
        The nearest network's node id for the given point, which is in the nearest network edge. 
    
  """
  #Reproject point coordinates from EPSG:4326 to the EPSG:3857.   
  point_x, point_y = pointreproject(point[0], point[1],'epsg:4326','epsg:3857')

  #Find the nearest road.
  _, _, _, nearest_road = ox.distance.get_nearest_edge(roadgraph, (point_y,point_x), return_geom=True)
  
  #Find all point coordinates that are constructing the specific road.
  nearest_road_coords = np.array(nearest_road.xy)

  #Calculate all distances between the input point and all the points constructing the nearest road line.
  e_distances = []
  for i in range(0,nearest_road_coords[0].size):
    e_dist = ox.distance.euclidean_dist_vec(point_y,point_x,nearest_road_coords[1][i],nearest_road_coords[0][i])
    e_distances.append(e_dist)
  mindist = min(e_distances)
  for i in range(0,len(e_distances)):
    if e_distances[i]==mindist: pos=i

  #Nearest edge node coordinates.
  nearest_edge_node_x = nearest_road_coords[0][pos]  
  nearest_edge_node_y = nearest_road_coords[1][pos]  

  #Find the nearest network's node id, which has the minimum distance from the road point that has the minimum distance from the input point.
  node_id= ox.distance.get_nearest_node(roadgraph, (nearest_edge_node_y,nearest_edge_node_x),  method='euclidean')

  return node_id

def pointreproject2edge(point,roadgraph):

  """
    Finds the nearest network's edge for the given point, and reprojects it perpendicularly to its nearest edge.
    If the orthogonal projection of the point lies outside the nearest edge's nodes, then the point isn't reprojected
    but it is transferred to the nearest network node instead.

     The given point's coordinates must be in the EPSG:4326, and following the form of (latitude, longitude).  

    Args:
        point: The point:(latitude, longitude) that needs to be projected to the nearest network edge.
        roadgraph: A MultiDiGraph (EPSG:3857) from the osmnx package.

    Returns:
        A tuple (latitude, longitude) of the reprojected coordinates of the input point.
    
  """

  #Reproject point coordinates from EPSG:4326 to the EPSG:3857.   
  point_x, point_y = pointreproject(point[0], point[1],'epsg:4326','epsg:3857')

  #Find the nearest road.
  _, _, _, nearest_road, dist__pp = ox.distance.get_nearest_edge(roadgraph, (point_y,point_x), return_geom=True, return_dist=True)

  #Extract the nearest road coordinates.
  nearest_road_coords = np.array(nearest_road.xy)

  #Calculate the euclidean distance between the input point and the nearest edge's first node.
  dist__pv = ox.distance.euclidean_dist_vec(point_y,point_x,nearest_road_coords[1][0],nearest_road_coords[0][0])

  #Calculate the hypotenuse.
  dist = np.sqrt(dist__pv**2-dist__pp**2)

  #Create the corresponding circle.
  circle = Point(nearest_road_coords[0][0], nearest_road_coords[1][0]).buffer(dist).boundary

  #Find the circle's intersection point with the given edge.
  intersection_points = circle.intersection(nearest_road)
  
  lies_in = False

  #If the orthogonal projection of the point lies inside the nearest edge's nodes.
  if type(intersection_points) == shapely.geometry.point.Point:
    lies_in = True
    intersection_point_x = intersection_points.x
    intersection_point_y = intersection_points.y
 
  #If the orthogonal projection of the point lies outside the nearest edge's nodes.
  else:
    gdf = ox.utils_graph.graph_to_gdfs(roadgraph)
    nodes_gdf = gdf[0].reset_index(drop=True) 
    nearest_node_id = nearest_edge_node_id(point,roadgraph)
    intersection_point_x = nodes_gdf.loc[(nodes_gdf['osmid']==nearest_node_id)].x.values[0]
    intersection_point_y = nodes_gdf.loc[(nodes_gdf['osmid']==nearest_node_id)].y.values[0]

  #Find the nearest node id from the intersection point.
  node_id = ox.distance.get_nearest_node(roadgraph, (intersection_point_y,intersection_point_x),  method='euclidean')

  #Reproject the calculated grid point from epsg:3857 to epsg:4326.
  point_lat, point_long = pointreproject(intersection_point_x, intersection_point_y,'epsg:3857','epsg:4326')

  return point_lat, point_long, lies_in

def routeplot(starting_point,ending_point):

  """
     Finds and plots the sortest route -based on Dijkstra's pathfinding algorithm- between the 
     two given points in the given roadgraph.The given points coordinates must be in the EPSG:4326 
     and they must be following the form of (latitude, longitude). 

    Args:
        starting_point: the route's starting point. -> A tuple of (latitude, longitude).
        ending_point: the route's ending point. -> A tuple of (latitude, longitude).

    Returns:
        A Folium map with the given starting and ending points and the shortest path between them. 
    
  """

  #Calculate the roadgraph between the starting and ending points.
  roadgraph = rgraphfrompoints(starting_point,ending_point)

  #Find the routes starting end ending point which lies inside the given roadgraph.
  p_lat1,p_long1,lies_in1 = pointreproject2edge(starting_point,roadgraph)
  p_lat2,p_long2,lies_in2 = pointreproject2edge(ending_point,roadgraph)

  #Find the nearest edge node id.
  node1_id = nearest_edge_node_id(starting_point,roadgraph)
  node2_id = nearest_edge_node_id(ending_point,roadgraph)
  
  #Calculate the sortest path between the two given nodes.
  nodeids = ox.shortest_path(roadgraph, node1_id, node2_id)

  #Create the necessary geodataframes.
  gdf = ox.utils_graph.graph_to_gdfs(roadgraph)
  nodes_gdf = gdf[0].reset_index(drop=True) 

  #Map's center point.
  center_lat = (starting_point[0]+ending_point[0])/2
  center_long = (starting_point[1]+ending_point[1])/2
  center = [center_lat,center_long]

  #Define the map.
  m = folium.Map(location=center, zoom_start=13, crs='EPSG3857')

  #Plot Original points.
  folium.Marker((starting_point[0], starting_point[1]), 
                 popup="Original Starting Point", tooltip="Original Starting Point", 
                 icon=folium.Icon(color='pink', icon="fas fa-car", prefix='fa')).add_to(m);

  folium.Marker((ending_point[0], ending_point[1]), 
                 popup="Original Destination", tooltip="Original Destination", 
                 icon=folium.Icon(color='pink', icon="fas fa-crosshairs", prefix='fa')).add_to(m);

  #Plot Starting and Destination points.
  folium.Marker((p_lat1,p_long1), 
                popup="Starting Point", tooltip="Starting Point", 
                icon=folium.Icon(color='darkblue', icon="fas fa-car", prefix='fa')).add_to(m);

  folium.Marker((p_lat2,p_long2), 
                popup="Destination", tooltip="Destination", 
                icon=folium.Icon(color='darkblue', icon="fas fa-crosshairs", prefix='fa')).add_to(m);

  #Create a list of tuples (locations) for each of route's node coordinates.
  locations = []
  locations.append((p_lat1,p_long1)) #Add the starting point to the route.
  for node_id in nodeids:
    p_lat = nodes_gdf.loc[(nodes_gdf['osmid']==node_id)].lat.values[0]
    p_long = nodes_gdf.loc[(nodes_gdf['osmid']==node_id)].lon.values[0]
    locations.append((p_lat,p_long))
  locations.append((p_lat2,p_long2)) #Add the ending point to the route.

  #Cut the final route segment if the starting point lies in it.
  if lies_in1: 
    r1 = pointreproject(locations[1][0],locations[1][1],'epsg:4326','epsg:3857')
    r2 = pointreproject(locations[2][0],locations[2][1],'epsg:4326','epsg:3857')
    road1 = LineString([r1,r2])
    point1_x, point1_y = pointreproject(p_lat1,p_long1,'epsg:4326','epsg:3857')
    point1 = Point(point1_x, point1_y)
    if road1.distance(point1) < 1e-8 :del locations[1] 

  #Cut the final route segment if the ending point lies in it.
  if lies_in2: 
    r3 = pointreproject(locations[-3][0],locations[-3][1],'epsg:4326','epsg:3857')
    r4 = pointreproject(locations[-2][0],locations[-2][1],'epsg:4326','epsg:3857')
    road2 = LineString([r3,r4])
    point2_x, point2_y = pointreproject(p_lat2,p_long2,'epsg:4326','epsg:3857')
    point2 = Point(point2_x, point2_y)
    if road2.distance(point2) < 1e-8 :del locations[-2] 


  #Create a folium polyline from the list of "locations".
  route = folium.PolyLine(locations=locations,tooltip='Route', smooth_factor=0.3, color="purple", weight=2.5, opacity=1)

  #Plot route.
  route.add_to(m);

  #Save map to html file.
  m.save('route.html') #saves the .html file in the local directory.

  #Plot map.
  return m

#Random points to WGS84 between latitude of 37.9000-38.0000 degrees and longitude of 23.7000-23.8000 degrees. (Athens, Attiki, Greece)
starting_point = (round(37.9+np.random.random()/10,4),round(23.7+np.random.random()/10,4))
ending_point = (round(37.9+np.random.random()/10,4),round(23.7+np.random.random()/10,4))

#Plot the route between starting and ending point.
routeplot(starting_point,ending_point)

#Print a .txt file with the code requirements.
!pip freeze > /content/drive/MyDrive/Projects/Python_Projects/Vehicle_Navigation-Sortest_Path/requirements.txt