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