def calcNewPathFromPathAndGrid(): new_path = [] curr_point = getGridPointFromPoint(start_point) total_length = 0 for action, point in ts_path: path_dist = point_cost_paths[pathPlanningHelpers.pointToString( curr_point)][pathPlanningHelpers.pointToString( getGridPointFromPoint(point))] total_length += path_dist[1] for grid_point in path_dist[0]: new_path.append([ waypoint_config["go command"], getRepPointFromGrid(grid_point) ]) # stop at last point new_path[-1][0] = waypoint_config["stop command"] curr_point = getGridPointFromPoint(point) # Calculate path from last point to start point last_action, last_coord = ts_path[-1] prev_last_action, prev_last_coord = ts_path[-2] calcDist(prev_last_coord, last_coord) path_dist = point_cost_paths[pathPlanningHelpers.pointToString( curr_point)][pathPlanningHelpers.pointToString( getGridPointFromPoint(start_point))] total_length += path_dist[1] for grid_point in path_dist[0]: new_path.append([ waypoint_config["go command"], getRepPointFromGrid(grid_point) ]) return new_path, total_length
def createGraph(start_point, sensor_nodes, obstacle_bboxes, board_limits): node_points = sensor_nodes.copy() node_points.append(start_point) lx, ly, ux, uy = board_limits graph = GeoHelpers.getGraphFromPointLimits(lx, ly, ux, uy, grid_size) # Find target nodes in which our node points are located start_point_coords = [(int((start_point[1] - lx) / (ux - lx) * grid_size)), int((start_point[0] - ly) / (uy - ly) * grid_size)] graph[pathPlanningHelpers.pointToString(start_point_coords)].append( "start") for sensor_node in sensor_nodes: graph[pathPlanningHelpers.pointToString([ int((sensor_node[1] - lx) / (ux - lx) * grid_size), int((sensor_node[0] - ly) / (uy - ly) * grid_size) ])].append("sensor") # Find nodes in which we have obstacles for bbox in obstacle_bboxes: ll, ur = [bbox[0], bbox[1]], [bbox[2], bbox[3]] bbox_lx = int((ll[1] - lx) / (ux - lx) * grid_size) bbox_ux = int((ur[1] - lx) / (ux - lx) * grid_size) bbox_ly = int((ll[0] - ly) / (uy - ly) * grid_size) bbox_uy = int((ur[0] - ly) / (uy - ly) * grid_size) for i in range(bbox_lx, bbox_ux + 1): for j in range(bbox_ly, bbox_uy + 1): graph[pathPlanningHelpers.pointToString([i, j ])].append("obstacle") # Keep the node points in the graph structure as a graph attribute graph["attributes"] = {"node points": node_points} return graph, start_point_coords
def getDynamicDist(first_point, second_point, groups): # Convert points to grid first_point_grid = getGridPointFromPoint(first_point) second_point_grid = getGridPointFromPoint(second_point) first_point_str = pathPlanningHelpers.pointToString(first_point_grid) second_point_str = pathPlanningHelpers.pointToString(second_point_grid) # Check if we already calculated this path previously if first_point_str in point_cost_paths and second_point_str in point_cost_paths[ first_point_str]: # We don't need to calculate new paths return point_cost_paths[first_point_str][second_point_str] else: # We need to build new graph and calculate paths and update them in point_cost_path dynamic_nodes = [] for group in groups: dynamic_nodes += [ GeoHelpers.getClosestPointFromPointToShape( Point(first_point[0], first_point[1]), group[0]) ] graph, start_point_coords = Graph.createGraph( first_point, dynamic_nodes, obstacle_bboxes, board_limits) graph = Graph.checkValidGrid(graph) point_cost_paths[first_point_str] = Graph.dijkstra( graph, first_point_grid) return point_cost_paths[first_point_str][second_point_str]
def getDist(first_point, second_point): # Convert points to grid first_point = getGridPointFromPoint(first_point) second_point = getGridPointFromPoint(second_point) # Find first point in point_cost_paths if pathPlanningHelpers.pointToString( first_point) not in point_cost_paths: point_cost_paths[pathPlanningHelpers.pointToString( first_point)] = Graph.dijkstra(graph, first_point) return point_cost_paths[pathPlanningHelpers.pointToString( first_point)][pathPlanningHelpers.pointToString(second_point)][1]
def calcDist(first_point, second_point): # Check if we can go in straight line first line = LineString([(first_point[0], first_point[1]), (second_point[0], second_point[1])]) first_point_grid, second_point_grid, first_point_str, second_point_str = getGridAndStringFromPoint( first_point, second_point) if not checkIntersectionWithObstacles(line): calcStraightLinePathBetweenPoints(first_point, second_point) return point_cost_paths[first_point_str] point_cost_paths[pathPlanningHelpers.pointToString( getGridPointFromPoint(first_point))] = Graph.dijkstra( graph, getGridPointFromPoint(first_point)) return point_cost_paths[pathPlanningHelpers.pointToString( getGridPointFromPoint(first_point))]
def dijkstra(graph, source): dist, prev, Q = {}, {}, [] for key in graph: dist[key] = float("inf") prev[key] = None Q.append(key) # Calculate targets dynamically. # targets = (start_point + sensor_points) - start_point_coords targets = [] dist[pathPlanningHelpers.pointToString(source)] = 0 while Q: u = getNodeWithMinDist(Q, dist, graph) # If only obstacle nodes are left if u is None: break Q.remove(u) if u != pathPlanningHelpers.pointToString(source) and ( graph[u] == 'sensor' or graph[u] == 'start'): targets.append(u) neighbors = list(getNeighbors(u, graph, Q)) for neighbor in neighbors: neighbor_coord, neighbor_dist = neighbor alt = dist[u] + neighbor_dist if alt < dist[pathPlanningHelpers.pointToString(neighbor_coord)]: dist[pathPlanningHelpers.pointToString(neighbor_coord)] = alt prev[pathPlanningHelpers.pointToString(neighbor_coord)] = u # Get shortest path to each of the other nodes from source node target_paths = {} for target in targets: S = [] u = target if prev[u] or u == source: while u: S.append(u) u = prev[u] S.reverse() target_paths[target] = [S, dist[target]] return target_paths
def getNeighbors(key, graph, Q): lx, ly, ux, uy = GeoHelpers.calcLimitsFromPoints( graph["attributes"]["node points"], path_planning_algorithm_config["Grid Coeff"]) x_step = (ux - lx) / grid_size y_step = (uy - ly) / grid_size x_dist, y_dist, diagonal_dist = GeoHelpers.calcDistances(x_step, y_step) # X neighbors node = [int(i) for i in key.split(',')] point = [node[0] + 1, node[1]] if point[0] < grid_size and graph[pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, x_dist] point = [node[0] - 1, node[1]] if point[0] >= 0 and graph[pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, x_dist] # Y neighbors point = [node[0], node[1] + 1] if point[1] < grid_size and graph[pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, y_dist] point = [node[0], node[1] - 1] if point[1] >= 0 and graph[pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, y_dist] # Diagonal neighbors point = [node[0] + 1, node[1] + 1] if point[0] < grid_size and point[1] < grid_size and graph[ pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, diagonal_dist] point = [node[0] + 1, node[1] - 1] if point[0] < grid_size and point[1] >= 0 and graph[ pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, diagonal_dist] point = [node[0] - 1, node[1] + 1] if point[0] >= 0 and point[1] < grid_size and graph[ pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, diagonal_dist] point = [node[0] - 1, node[1] - 1] if point[0] >= 0 and point[1] >= 0 and graph[ pathPlanningHelpers.pointToString( point)] != 'obstacle' and pathPlanningHelpers.pointToString( point) in Q: yield [point, diagonal_dist]
def findPathRecursively(groups, curr_path=None, curr_path_dist=0, new_point_radius_list=point_radius_list, curr_point=start_point, curr_stop_points=[]): nonlocal path_dist, path, stop_points if not curr_path: curr_path = [ pathPlanningHelpers.pointToString( extra_funcs_hash["Point to grid"](start_point)) ] tmp_curr_path = curr_path.copy() tmp_curr_path_dist = curr_path_dist tmp_new_point_radius_list = new_point_radius_list.copy() tmp_curr_stop_points = curr_stop_points.copy() for group in groups: next_point = GeoHelpers.getClosestPointFromPointToShape( Point(curr_point[0], curr_point[1]), group[0]) path_and_cost_to_next_point = mapping_function( curr_point, GeoHelpers.getClosestPointFromPointToShape( Point(curr_point[0], curr_point[1]), group[0]), groups) if len(path_and_cost_to_next_point[0]) > 1: curr_path += path_and_cost_to_next_point[0][1:] else: curr_path += path_and_cost_to_next_point[0] curr_stop_points.append(path_and_cost_to_next_point[0][-1]) curr_path_dist += path_and_cost_to_next_point[1] if curr_path_dist > path_dist: curr_path = tmp_curr_path.copy() curr_path_dist = tmp_curr_path_dist new_point_radius_list = tmp_new_point_radius_list.copy() curr_stop_points = tmp_curr_stop_points.copy() continue # Recalculate groups based on what we removed and go in recursively for group_lm in group[1]: for point in new_point_radius_list: point_obj = Point(point[0], point[1]) circle = point_obj.buffer(point[2]) if circle.within(group_lm): new_point_radius_list.remove(point) groups_alt = GeoHelpers.getShapeGroups(new_point_radius_list) groups_alt += GeoHelpers.getCirclesNotInIntersections( new_point_radius_list) if not groups_alt: # Calculate path from last point to start point path_to_start, cost_to_start = extra_funcs_hash[ "Calc Dist"](next_point, start_point)[curr_path[0]] if len(path_to_start) > 1: curr_path += path_to_start[1:] else: curr_path += path_to_start curr_path_dist += cost_to_start if curr_path_dist < path_dist: path_dist = curr_path_dist path = curr_path.copy() stop_points = curr_stop_points.copy() return findPathRecursively(groups_alt.copy(), curr_path.copy(), curr_path_dist, new_point_radius_list.copy(), next_point, curr_stop_points) curr_path = tmp_curr_path.copy() curr_path_dist = tmp_curr_path_dist new_point_radius_list = tmp_new_point_radius_list.copy() curr_stop_points = tmp_curr_stop_points.copy()
def getGridAndStringFromPoint(first_point, second_point): first_point_grid = getGridPointFromPoint(first_point) second_point_grid = getGridPointFromPoint(second_point) first_point_str = pathPlanningHelpers.pointToString(first_point_grid) second_point_str = pathPlanningHelpers.pointToString(second_point_grid) return first_point_grid, second_point_grid, first_point_str, second_point_str