import matplotlib.pyplot as plt import numpy as np import math from shapely.geometry import Point, LineString, Polygon import geopandas as gpd import networkx as nx from geopy import Point from geopy.distance import geodesic from gardens.models import DroneStations, Missions, FlightPlans, Blocks, Trees, TreeRow, ProtectedRegions # array structure for tree coordinates: # [0] - latitude # [1] - longitude # [2] - altitude # [3] - is_a_corner_tree # [4] - tree_id # [5] - row_id # path point array structure: # [0] - latitude # [1] - longitude # [2] - altitude # [3] - angle # [4] - tree_id # [5] - row_id # [6] - is_a_corner_tree # [7] - is_on_the_left_side_of_the_row def path_calculations(block_id, flying_altitude = 2, max_distance_to_tree_meters = 2, obstacle_avoidance = True, only_sides = False): # get the coordinates from the database trees_from_database = Trees.objects.filter(block_id=block_id, row_id__isnull=False).order_by('row_id', 'id') if not trees_from_database: raise Exception("Šajā kvartālā nav neviena koka.") try: DroneStation = DroneStations.objects.get(block_id = block_id) except DroneStations.DoesNotExist: raise Exception("Nav pievienota drona stacija.") garden_id = Blocks.objects.get(id = block_id).garden_id obstaclesDataBase = ProtectedRegions.objects.filter(garden_id = garden_id) obstacles = [] # geojson format for obstacle in obstaclesDataBase: obstacleGEOJSON = obstacle.polygon # convert the geojson format to a numpy array obstacleNoGEOJSON = np.array(obstacleGEOJSON['geometry']['coordinates'][0]) # change the order of the points so that the first point is x and the second point is y obstacleNoGEOJSON = np.flip(obstacleNoGEOJSON, axis=1) # add the obstacle to the obstacles array obstacles.append(obstacleNoGEOJSON) obstacles = np.array(obstacles) max_distance_to_tree_coordinates = max_distance_to_tree_meters / 111111 starting_point = [DroneStation.longitude, DroneStation.latitude] def find_first_tree_in_row(row_id): lowest_id = float('inf') for tree in trees_from_database: if tree.row_id == row_id: if tree.id < lowest_id: lowest_id = tree.id return lowest_id def convert_from_json(): tree_coordinates = [] for tree in trees_from_database: if tree.id == find_first_tree_in_row(tree.row_id): tree_coordinates.append([tree.longitude, tree.latitude, 2, 1, tree.id, tree.row_id]) elif tree.id == find_first_tree_in_row(tree.row_id) + 1: tree_coordinates.append([tree.longitude, tree.latitude, 2, 1, tree.id, tree.row_id]) else: tree_coordinates.append([tree.longitude, tree.latitude, 2, 0, tree.id, tree.row_id]) return tree_coordinates def find_tree_row_ids(): tree_row_ids = [] for tree in trees_from_database: if tree.row_id not in tree_row_ids: tree_row_ids.append(tree.row_id) return tree_row_ids def calculate_initial_compass_bearing(pointA, pointB): if (type(pointA) != tuple) or (type(pointB) != tuple): raise TypeError("Only tuples are supported as arguments") lat1 = math.radians(pointA[0]) lat2 = math.radians(pointB[0]) diffLong = math.radians(pointB[1] - pointA[1]) x = math.sin(diffLong) * math.cos(lat2) y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) * math.cos(lat2) * math.cos(diffLong)) initial_bearing = math.atan2(x, y) # Now we have the initial bearing but math.atan2() returns values from -π to + π so we need to normalize the result initial_bearing = math.degrees(initial_bearing) compass_bearing = (initial_bearing + 360) % 360 return compass_bearing def get_angle_between_two_points(point1, point2): point1 = (point1[0], point1[1]) point2 = (point2[0], point2[1]) bearing = calculate_initial_compass_bearing(point1, point2) return bearing def tree_row_angle(row_id): first_tree = [] last_tree = [] for coordinate in trees_from_database: if coordinate.id == find_first_tree_in_row(row_id): first_tree = [coordinate.longitude, coordinate.latitude] if coordinate.id == find_first_tree_in_row(row_id) + 1: last_tree = [coordinate.longitude, coordinate.latitude] return get_angle_between_two_points(first_tree, last_tree) def create_point_left(point): x = point[0] y = point[1] bearing = 0 for row in row_id_angles: if row['row_id'] == point[5]: bearing = row['angle'] tree_x = tree_coordinates[tree_coordinates[:,4] == point[4]][0][0] tree_y = tree_coordinates[tree_coordinates[:,4] == point[4]][0][1] point_new = geodesic(meters=max_distance_to_tree_meters).destination(Point(x,y), bearing + 90) x_new = point_new.latitude y_new = point_new.longitude angle_to_tree = get_angle_between_two_points([x_new,y_new],[tree_x,tree_y]) return [x_new, y_new, flying_altitude, angle_to_tree, point[4], point[5], False, True] def create_point_right(point): x = point[0] y = point[1] bearing = 0 for row in row_id_angles: if row['row_id'] == point[5]: bearing = row['angle'] tree_x = tree_coordinates[tree_coordinates[:,4] == point[4]][0][0] tree_y = tree_coordinates[tree_coordinates[:,4] == point[4]][0][1] angle_to_tree = get_angle_between_two_points([x,y],[tree_x,tree_y]) point_new = geodesic(meters=max_distance_to_tree_meters).destination(Point(x,y), bearing - 90) x_new = point_new.latitude y_new = point_new.longitude angle_to_tree = get_angle_between_two_points([x_new,y_new],[tree_x,tree_y]) return [x_new, y_new, flying_altitude, angle_to_tree, point[4], point[5], False, False] def create_corner_point(point,other_point,max_distance_to_tree_coordinates): x = point[0] y = point[1] dx = point[0] - other_point[0] dy = point[1] - other_point[1] angle = math.atan2(dy, dx) tree_x = tree_coordinates[tree_coordinates[:,4] == point[4]][0][0] tree_y = tree_coordinates[tree_coordinates[:,4] == point[4]][0][1] angle_to_tree = get_angle_between_two_points([x,y],[tree_x,tree_y]) x = point[0] + max_distance_to_tree_coordinates * math.cos(angle) y = point[1] + max_distance_to_tree_coordinates * math.sin(angle) return [x + max_distance_to_tree_coordinates * math.cos(angle), y + max_distance_to_tree_coordinates * math.sin(angle), flying_altitude, angle_to_tree, point[4], point[5], True, False] def create_tree_rows_outer_path(tree_coordinates): tree_path_points = [] for i in range(len(tree_coordinates)): if tree_coordinates[i][3] != True: tree_path_points.append(create_point_left(tree_coordinates[i])) tree_path_points.append(create_point_right(tree_coordinates[i])) else: tree_path_points.append(create_corner_point(tree_coordinates[i],tree_coordinates[i+1],max_distance_to_tree_coordinates)) tree_path_points.append(create_point_left(tree_coordinates[i])) tree_path_points.append(create_point_right(tree_coordinates[i])) return np.array(tree_path_points) # find the closest point to the starting point for each row def find_closest_point_to_starting_point(row_id): points = [] smallest_distance = float('inf') row_endpoints = corner_points[corner_points[:,5] == row_id] for i in row_endpoints: distance = math.sqrt((starting_point[0] - i[0])**2 + (starting_point[1] - i[1])**2) points.append({ 'point': i[0], 'distance': distance}) if distance < smallest_distance: smallest_distance = distance closest_point = i #print("Points: ", points) return closest_point def find_closest_point(point, points_left): smallest_distance = float('inf') for i in points_left: distance = math.sqrt((point[0] - i[0])**2 + (point[1] - i[1])**2) # haversine_distance = hs.haversine((point[0], point[1]), (i[0], i[1])) if distance < smallest_distance: smallest_distance = distance closest_point = i return closest_point # create the shortest path for each row with the closest point as the starting point and the end point of the path def create_shortest_path_flying_around(row_id): path = [] row_points = tree_path_points[tree_path_points[:,5] == row_id] starting_corner_point = find_closest_point_to_starting_point(row_id) leftside_points = row_points[row_points[:,7] == 0] rightside_points = row_points[row_points[:,7] == 1] corner_point = row_points[row_points[:,6] == True] path.append(starting_corner_point) for i in range(len(leftside_points)): path.append(find_closest_point(path[-1], leftside_points)) leftside_points = leftside_points[leftside_points[:,0] != path[-1][0]] unique_corner_points = set(tuple(x) for x in corner_point) unique_corner_points = np.array(list(unique_corner_points)) if len(unique_corner_points) > 0: if unique_corner_points[0][0] == starting_corner_point[0] and unique_corner_points[0][1] == starting_corner_point[1]: corner_point = unique_corner_points[1] path.append(corner_point) else: corner_point = unique_corner_points[0] path.append(corner_point) for i in range(len(rightside_points)): path.append(find_closest_point(path[-1], rightside_points)) rightside_points = rightside_points[rightside_points[:,0] != path[-1][0]] path.append(starting_corner_point) return path def create_shortest_path_flying_left_side_only(row_id): path = [] row_points = tree_path_points[tree_path_points[:,5] == row_id] starting_corner_point = find_closest_point_to_starting_point(row_id) leftside_points = row_points[row_points[:,7] == 0] leftside_points = leftside_points[leftside_points[:,6] == False] path.append(starting_corner_point) for i in range(len(leftside_points)): if len(path) == 2: first_side_point = path[-1] path.append(find_closest_point(path[-1], leftside_points)) leftside_points = leftside_points[leftside_points[:,0] != path[-1][0]] path.append(first_side_point) path.append(starting_corner_point) return path def create_shortest_path_flying_right_side_only(row_id): path = [] row_points = tree_path_points[tree_path_points[:,5] == row_id] starting_corner_point = find_closest_point_to_starting_point(row_id) rightside_points = row_points[row_points[:,7] == 1] rightside_points = rightside_points[rightside_points[:,6] == False] path.append(starting_corner_point) for i in range(len(rightside_points)): if len(path) == 2: first_side_point = path[-1] path.append(find_closest_point(path[-1], rightside_points)) rightside_points = rightside_points[rightside_points[:,0] != path[-1][0]] path.append(first_side_point) path.append(starting_corner_point) return path def create_first_tree_path(tree_row_ids_array, only_sides = False): shortest_paths = [] for row_id in tree_row_ids_array: if only_sides == False: shortest_paths.append(create_shortest_path_flying_around(row_id)) else: shortest_paths.append(create_shortest_path_flying_left_side_only(row_id)) shortest_paths.append(create_shortest_path_flying_right_side_only(row_id)) shortest_paths = np.array(shortest_paths, dtype=object) return shortest_paths def intersects(s0,s1): dx0 = s0[1][0]-s0[0][0] dx1 = s1[1][0]-s1[0][0] dy0 = s0[1][1]-s0[0][1] dy1 = s1[1][1]-s1[0][1] p0 = dy1*(s1[1][0]-s0[0][0]) - dx1*(s1[1][1]-s0[0][1]) p1 = dy1*(s1[1][0]-s0[1][0]) - dx1*(s1[1][1]-s0[1][1]) p2 = dy0*(s0[1][0]-s1[0][0]) - dx0*(s0[1][1]-s1[0][1]) p3 = dy0*(s0[1][0]-s1[1][0]) - dx0*(s0[1][1]-s1[1][1]) return (p0*p1<=0) & (p2*p3<=0) # this function finds the best path between two points while avoiding an obstacle def avoid_obstacles(point1, point2, obstacles): G = nx.Graph() # add the starting point and the end point to the graph G.add_node((point1[0], point1[1])) G.add_node((point2[0], point2[1])) # create points that are around the obstacle for obstacle in obstacles: geopandas_obstacle = gpd.GeoSeries(Polygon(obstacle)) # points that are around the obstacle that are gonna be used to create a path scaled_obstacle_points = geopandas_obstacle.scale(xfact=1.1 , yfact=1.1, origin='center') # the points are stored in a geopandas series, so we need to convert them to a numpy array having latitude be the first column and longitude the second column scaled_obstacle_points = np.array(scaled_obstacle_points[0].exterior.coords) # change the order of the points so that the first point is x and the second point is y scaled_obstacle_points = np.flip(scaled_obstacle_points, axis=1) # remove the last point because it is the same as the first point scaled_obstacle_points = scaled_obstacle_points[:-1] # use obstacle points to create a path from point 1 to point 2 using dijkstra algorithm # add nodes to the graph for i in scaled_obstacle_points: G.add_node((i[0], i[1])) # add edges to the graph for i in range(len(scaled_obstacle_points)): for j in range(len(scaled_obstacle_points)): if i != j: # calculate the distance between the two points distance = math.sqrt((scaled_obstacle_points[i][0] - scaled_obstacle_points[j][0])**2 + (scaled_obstacle_points[i][1] - scaled_obstacle_points[j][1])**2) G.add_edge((scaled_obstacle_points[i][0], scaled_obstacle_points[i][1]), (scaled_obstacle_points[j][0], scaled_obstacle_points[j][1]), weight=distance) # add nodes for each corner point in the graph that are not the end point for i in corner_points: if i[0] != point2[0] and i[1] != point2[1]: G.add_node((i[0], i[1])) # add edges for each node that exists in the graph for i in G.nodes: for j in G.nodes: if i != j: distance = math.sqrt((i[0] - j[0])**2 + (i[1] - j[1])**2) G.add_edge((i[0], i[1]), (j[0], j[1]), weight=distance) # remove edges that interfere with the obstacles edges_to_remove = [] for edge in G.edges(): for obstacle in obstacles: for i in range(len(obstacle)): if intersects(edge, [(obstacle[i][1], obstacle[i][0]), (obstacle[i-1][1], obstacle[i-1][0])]): edges_to_remove.append(edge) G.remove_edges_from(edges_to_remove) # remove edges that interfere with the tree row edges_to_remove = [] for edge in G.edges(): for row_id in tree_row_ids: first_tree_id = find_first_tree_in_row(row_id) last_tree_id = find_first_tree_in_row(row_id) + 1 first_tree = tree_coordinates[tree_coordinates[:,4] == first_tree_id][0] last_tree = tree_coordinates[tree_coordinates[:,4] == last_tree_id][0] if intersects(edge, [(first_tree[0], first_tree[1]), (last_tree[0], last_tree[1])]): edges_to_remove.append(edge) #plt.plot([edge[0][0], edge[1][0]], [edge[0][1], edge[1][1]], color='blue', linewidth=1.0, linestyle='--',alpha=0.5) #plt.plot([first_tree[0], last_tree[0]], [first_tree[1], last_tree[1]], color='red', linewidth=1.0, linestyle='--',alpha=0.5) G.remove_edges_from(edges_to_remove) # find the shortest path between the starting point and the end point shortest_path = nx.dijkstra_path(G, (point1[0], point1[1]), (point2[0], point2[1])) # check if the going to the other side of the row is shorter row_id = point2[5] first_point = find_closest_point_to_starting_point(row_id) second_point = [] for i in corner_points: if i[0] != first_point[0] and i[5] == row_id and i[1] != first_point[1]: second_point = [i[0],i[1]] second_shortest_path = nx.dijkstra_path(G, (point1[0], point1[1]), (second_point[0], second_point[1])) # check distance between the two paths first_path_distance = 0 second_path_distance = 0 for i in range(len(shortest_path) - 1): first_path_distance += math.sqrt((shortest_path[i][0] - shortest_path[i+1][0])**2 + (shortest_path[i][1] - shortest_path[i+1][1])**2) for i in range(len(second_shortest_path) - 1): second_path_distance += math.sqrt((second_shortest_path[i][0] - second_shortest_path[i+1][0])**2 + (second_shortest_path[i][1] - second_shortest_path[i+1][1])**2) # make the shortest path into a numpy array that can be added to the start of clean_path_row shortest_path = np.array(shortest_path) return shortest_path def obstacle_avoidance_path(starting_point, obstacles, clean_path_row): row_id = clean_path_row[0][5] closest_point = closest_points[closest_points[:,5] == row_id][0] points_to_clean_path_row = avoid_obstacles(starting_point, closest_point, obstacles) return points_to_clean_path_row def final_paths_with_obstacle_avoidance(tree_row_path, starting_point): clean_path = np.copy(tree_row_path) # check array size of clean_path if clean_path.shape[0] == 1: row_id = clean_path[0][0][5] start_of_path = obstacle_avoidance_path(starting_point, obstacles, clean_path[0]) for j in range(len(start_of_path)): latitude = start_of_path[j][0] longitude = start_of_path[j][1] point = np.array([[latitude, longitude, flying_altitude, 0, 0, row_id, 0, 0]]) clean_path = np.insert(clean_path, j, point, axis=1) # add the start_of_path in reverse at the end of the clean_path reverse_start_of_path = np.flip(start_of_path, axis=0) for j in range(len(reverse_start_of_path)): latitude = reverse_start_of_path[j][0] longitude = reverse_start_of_path[j][1] point = np.array([[latitude, longitude, flying_altitude, 0, 0, row_id, 0, 0]]) clean_path = np.insert(clean_path, len(clean_path[0]), point, axis=1) # add the starting point at the beginning of the clean_path and at the end of the clean_path starting_point_x = starting_point[0] starting_point_y = starting_point[1] starting_point_altitude = 0 starting_point_angle = 0 starting_point = [starting_point_x, starting_point_y, starting_point_altitude, starting_point_angle, 0, 0, 0, 0] clean_path = np.insert(clean_path, 0, starting_point, axis=1) clean_path = np.insert(clean_path, len(clean_path), starting_point, axis=1) return clean_path for i in range(len(clean_path)): row_id = clean_path[i][0][5] start_of_path = obstacle_avoidance_path(starting_point, obstacles, clean_path[i]) for j in range(len(start_of_path)): latitude = start_of_path[j][0] longitude = start_of_path[j][1] point = np.array([[latitude, longitude, flying_altitude, 0, 0, row_id, 0, 0]]) clean_path[i] = np.insert(clean_path[i], j, point, axis=0) # add the start_of_path in reverse at the end of the clean_path reverse_start_of_path = np.flip(start_of_path, axis=0) for j in range(len(reverse_start_of_path)): latitude = reverse_start_of_path[j][0] longitude = reverse_start_of_path[j][1] point = np.array([[latitude, longitude, flying_altitude, 0, 0, row_id, 0, 0]]) clean_path[i] = np.insert(clean_path[i], len(clean_path[i]), point, axis=0) # add the starting point at the beginning of the clean_path and at the end of the clean_path starting_point_x = starting_point[0] starting_point_y = starting_point[1] starting_point_altitude = 0 starting_point_angle = 0 starting_point = [starting_point_x, starting_point_y, starting_point_altitude, starting_point_angle, 0, 0, 0, 0] clean_path[i] = np.insert(clean_path[i], 0, starting_point, axis=0) clean_path[i] = np.insert(clean_path[i], len(clean_path[i]), starting_point, axis=0) return clean_path def final_paths_with_no_obstacle_avoidance(tree_row_path, starting_point, obstacle_avoidance_altitude = 10, ): clean_path_altitude = np.copy(tree_row_path) #altitude_to fly above the obstacles if clean_path_altitude.shape[0] == 1: # add the starting point to the path starting_point_x = starting_point[0] starting_point_y = starting_point[1] starting_point_altitude = 0 starting_point_angle = 0 starting_point = [starting_point_x, starting_point_y, starting_point_altitude, starting_point_angle, 0, 0, 0, 0] clean_path_altitude = np.insert(clean_path_altitude, 0 ,starting_point, axis=1) # add the starting point but at a higher altitude to the path starting_point_altitude = obstacle_avoidance_altitude point = [starting_point_x, starting_point_y, starting_point_altitude, starting_point_angle, 0, 0, 0, 0] # add the corner point to the path corner_point_x = clean_path_altitude[0][1][0] corner_point_y = clean_path_altitude[0][1][1] corner_point = [corner_point_x, corner_point_y, obstacle_avoidance_altitude, 0, 0, 0, 0, 0] clean_path_altitude = np.insert(clean_path_altitude, 1 ,corner_point, axis=1) # add the end point to the path clean_path_altitude = np.insert(clean_path_altitude, len(clean_path_altitude), corner_point, axis=1) clean_path_altitude = np.insert(clean_path_altitude, len(clean_path_altitude), point, axis=1) clean_path_altitude = np.insert(clean_path_altitude, len(clean_path_altitude) ,starting_point, axis=1) return clean_path_altitude for i in range(len(clean_path_altitude)): # add the starting point to the path starting_point_x = starting_point[0] starting_point_y = starting_point[1] starting_point_altitude = 0 starting_point_angle = 0 starting_point = [starting_point_x, starting_point_y, starting_point_altitude, starting_point_angle, 0, 0, 0, 0] clean_path_altitude[i] = np.insert(clean_path_altitude[i], 0 ,starting_point, axis=0) # add the starting point but at a higher altitude to the path starting_point_altitude = obstacle_avoidance_altitude point = [starting_point_x, starting_point_y, starting_point_altitude, starting_point_angle, 0, 0, 0, 0] # add the corner point to the path corner_point_x = clean_path_altitude[i][1][0] corner_point_y = clean_path_altitude[i][1][1] corner_point = [corner_point_x, corner_point_y, obstacle_avoidance_altitude, 0, 0, 0, 0, 0] clean_path_altitude[i] = np.insert(clean_path_altitude[i], 1 ,corner_point, axis=0) # add the end point to the path clean_path_altitude[i] = np.insert(clean_path_altitude[i], len(clean_path_altitude[i]), corner_point, axis=0) clean_path_altitude[i] = np.insert(clean_path_altitude[i], len(clean_path_altitude[i]), point, axis=0) clean_path_altitude[i] = np.insert(clean_path_altitude[i], len(clean_path_altitude[i]) ,starting_point, axis=0) return clean_path_altitude def calculate_flight_time_altitude(flight_path, speed=2, ascent_speed=2, descent_speed=1): total_distance = 0 total_time = 0 total_altitude_distance = 0 for i in range(len(flight_path) - 1): distance = math.sqrt((flight_path[i][0] - flight_path[i+1][0])**2 + (flight_path[i][1] - flight_path[i+1][1])**2) distance = distance * 111111 distance_altitude = flight_path[i][2] - flight_path[i+1][2] total_distance += distance total_altitude_distance += abs(distance_altitude) if distance_altitude > 0: time = distance_altitude / ascent_speed else: time = abs(distance_altitude) / descent_speed total_time += time total_time += total_distance / speed return total_time def compare_final_paths_by_time(clean_path, clean_path_altitude): final_path = [] for i in range(len(clean_path)): time = calculate_flight_time_altitude(clean_path[i]) time_altitude = calculate_flight_time_altitude(clean_path_altitude[i]) if time < time_altitude: final_path.append(clean_path[i]) else: final_path.append(clean_path_altitude[i]) return final_path tree_coordinates = np.array(convert_from_json()) tree_row_ids = find_tree_row_ids() row_id_angles = [] for row_id in tree_row_ids: row_id_angles.append({ 'row_id': row_id, 'angle' : tree_row_angle(row_id)}) tree_path_points = create_tree_rows_outer_path(tree_coordinates) corner_points = [] for i in tree_path_points: if i[6] == True: corner_points.append(i) corner_points = np.array(corner_points) # attempt to find the closest points closest_points = [] for row_id in tree_row_ids: closest_points.append(find_closest_point_to_starting_point(row_id)) closest_points = np.array(closest_points) tree_row_paths = create_first_tree_path(tree_row_ids, only_sides) paths_with_obstacle_avoidance = final_paths_with_obstacle_avoidance(tree_row_paths, starting_point) paths_with_no_obstacle_avoidance = final_paths_with_no_obstacle_avoidance(tree_row_paths, starting_point) final_paths = compare_final_paths_by_time(paths_with_obstacle_avoidance, paths_with_no_obstacle_avoidance) if obstacle_avoidance == True: final_paths = paths_with_obstacle_avoidance return final_paths