planning_utils.py

import math
from enum import Enum
from queue import PriorityQueue
import numpy as np


def create_grid(data, drone_altitude, safety_distance):
    """
    Returns a grid representation of a 2D configuration space
    based on given obstacle data, drone altitude and safety distance
    arguments.
    """

    # minimum and maximum north coordinates
    north_min = np.floor(np.min(data[:, 0] - data[:, 3]))
    north_max = np.ceil(np.max(data[:, 0] + data[:, 3]))

    # minimum and maximum east coordinates
    east_min = np.floor(np.min(data[:, 1] - data[:, 4]))
    east_max = np.ceil(np.max(data[:, 1] + data[:, 4]))

    # given the minimum and maximum coordinates we can
    # calculate the size of the grid.
    north_size = int(np.ceil(north_max - north_min))
    east_size = int(np.ceil(east_max - east_min))

    # Initialize an empty grid
    grid = np.zeros((north_size, east_size))

    # Populate the grid with obstacles
    for i in range(data.shape[0]):
        north, east, alt, d_north, d_east, d_alt = data[i, :]
        if alt + d_alt + safety_distance > drone_altitude:
            obstacle = [
                int(np.clip(north - d_north - safety_distance - north_min, 0, north_size - 1)),
                int(np.clip(north + d_north + safety_distance - north_min, 0, north_size - 1)),
                int(np.clip(east - d_east - safety_distance - east_min, 0, east_size - 1)),
                int(np.clip(east + d_east + safety_distance - east_min, 0, east_size - 1)),
            ]
            grid[obstacle[0]:obstacle[1] + 1, obstacle[2]:obstacle[3] + 1] = 1

    return grid, int(north_min), int(east_min)


# Assume all actions cost the same.
class Action(Enum):
    """
    An action is represented by a 3 element tuple.

    The first 2 values are the delta of the action relative
    to the current grid position. The third and final value
    is the cost of performing the action.
    """

    WEST = (0, -1, 1)
    EAST = (0, 1, 1)
    NORTH = (-1, 0, 1)
    SOUTH = (1, 0, 1)

    @property
    def cost(self):
        return self.value[2]

    @property
    def delta(self):
        return (self.value[0], self.value[1])


def valid_actions(grid, current_node):
    """
    Returns a list of valid actions given a grid and current node.
    """
    valid_actions = list(Action)
    n, m = grid.shape[0] - 1, grid.shape[1] - 1
    x, y = current_node

    # check if the node is off the grid or
    # it's an obstacle

    if x - 1 < 0 or grid[x - 1, y] == 1:
        valid_actions.remove(Action.NORTH)
    if x + 1 > n or grid[x + 1, y] == 1:
        valid_actions.remove(Action.SOUTH)
    if y - 1 < 0 or grid[x, y - 1] == 1:
        valid_actions.remove(Action.WEST)
    if y + 1 > m or grid[x, y + 1] == 1:
        valid_actions.remove(Action.EAST)

    return valid_actions


def a_star(grid, h, start, goal):
    path = []
    path_cost = 0
    queue = PriorityQueue()
    queue.put((0, start))
    visited = set(start)

    branch = {}
    found = False

    while not queue.empty():
        item = queue.get()
        current_node = item[1]
        if current_node == start:
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]

        if current_node == goal:
            found = True
            break
        else:
            for action in valid_actions(grid, current_node):
                # get the tuple representation
                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1])
                branch_cost = current_cost + action.cost
                queue_cost = branch_cost + h(next_node, goal)

                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))

    if found:
        # retrace steps
        n = goal
        path_cost = branch[n][0]
        path.append(goal)
        while branch[n][1] != start:
            path.append(branch[n][1])
            n = branch[n][1]
        path.append(branch[n][1])
        print('Found a path from {} to {} with cost: {}'.format(start, goal, path_cost))
    else:
        print('**********************')
        print('Failed to find a path!')
        print('**********************')
    return path[::-1], path_cost

def iterative_astar(grid, h, start, goal):
    """
        Implement the iterative deepening A* algorithm by continuously looking
        for goal node using dfs approach with increasing depth limit: 0, 1, ...
        until find the goal node.
    """
    def dfs(grid, h, current_node, start, goal, depth_limit, visited, branch):
        """
            Implement the dfs algorithm for the depth of depth_limit
        """
        path = []
        path_cost = 0
        found = False
        found_in_sub = False
        if current_node == start:
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]
        # Cut the branch with f(current, goal) > depth_limit
        if current_cost + h(current_node, goal) > depth_limit:
            found = False
        # Check found when current_cost == depth_limit
        elif current_cost == depth_limit:
            found = (current_node == goal)
        # Check whether current_node == goal
        elif current_node == goal:
            found = True
        # Check whether currnt_node != goal
        else:
            # Check the child node
            for action in valid_actions(grid, current_node):
                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1])
                branch_cost = current_cost + action.cost
                # if the node is not visited, then add it to visited, and repeat
                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    path, path_cost, found = dfs(grid,
                                                 h,
                                                 next_node,
                                                 start,
                                                 goal,
                                                 depth_limit,
                                                 visited,
                                                 branch)
                    if found:
                        # prevent repeatedly print the result
                        found_in_sub = True
                        break
        if found and not found_in_sub:
            # retrace steps
            n = goal
            path_cost = branch[n][0]
            path.append(goal)
            while branch[n][1] != start:
                path.append(branch[n][1])
                n = branch[n][1]
            path.append(branch[n][1])
        return path, path_cost, found
    # Iterative deepening process
    found = False
    current_limit = 0
    # iterative the deepening process until the goal is found
    while not found:
        visited = set(start)
        branch = {}
        path, path_cost, found = dfs(grid, h, start, start, goal, current_limit, visited, branch)
        if found:
            print('Found a path from {} to {} with cost: {}'.format(start, goal, path_cost))
            print("Found a path at current limit: ", current_limit)
            return path[::-1], path_cost
        # Otherwise, Failed to find the goal at current depth limit
        current_limit = current_limit + 1

def a_start_to_traverse_three_fixed_point(grid, h, start, goal, node_1, node_2, node_3):
    """
        Implement the a_star to find the shortest path from start to goal and traverse
        the three nodes
    """
    def a_star_modified(grid, h, start, goal):
        path = []
        path_cost = 0
        queue = PriorityQueue()
        queue.put((0, start))
        visited = set(start)
        branch = {}
        found = False
        while not queue.empty():
            item = queue.get()
            current_node = item[1]
            if current_node == start:
                current_cost = 0.0
            else:
                current_cost = branch[current_node][0]
            if current_node == goal:
                found = True
                break
            else:
                for action in valid_actions(grid, current_node):
                    # get the tuple representation
                    da = action.delta
                    next_node = (current_node[0] + da[0], current_node[1] + da[1])
                    branch_cost = current_cost + action.cost
                    queue_cost = branch_cost + h(next_node, goal)

                    if next_node not in visited:
                        visited.add(next_node)
                        branch[next_node] = (branch_cost, current_node, action)
                        queue.put((queue_cost, next_node))
        if found:
            # retrace steps
            n = goal
            path_cost = branch[n][0]
            path.append(goal)
            while branch[n][1] != start:
                path.append(branch[n][1])
                n = branch[n][1]
            path.append(branch[n][1])
        return path[::-1], path_cost, found
    path = []
    path_cost = math.inf
    node_list = [node_1, node_2, node_3]
    for first_node in node_list:
        cp_node_list = node_list
        cp_node_list.remove(first_node)
        for second_node in cp_node_list:
            cp_node_list.remove(second_node)
            third_node = cp_node_list[0]
            tmp_path0, tmp_path_cost0, tmp_found_0 = a_star_modified(grid, h, start, first_node)
            tmp_path1, tmp_path_cost1, tmp_found_1 = a_star_modified(grid, h, first_node, second_node)
            tmp_path2, tmp_path_cost2, tmp_found_2 = a_star_modified(grid, h, second_node, third_node)
            tmp_path3, tmp_path_cost3, tmp_found_3 = a_star_modified(grid, h, third_node, goal)
            if tmp_found_0 and tmp_found_1 and tmp_found_2 and tmp_found_3:
                tmp_path = tmp_path0 + tmp_path1 + tmp_path2 + tmp_path3
                tmp_path_cost = tmp_path_cost0 + tmp_path_cost1 + tmp_path_cost2 + tmp_path_cost3
                if tmp_path_cost < path_cost:
                    path = tmp_path
                    path_cost = tmp_path_cost
    return path, path_cost

def heuristic_manhattan(position, goal_position):
    """
        Implement the heuristic function by calculating the Manhattan Distance
    """
    return abs(position[0] - goal_position[0]) + abs(position[1] - goal_position[1])

def heuristic_chebyshev(position, goal_position):
    """
        Implement the heuristic function by calculating the Chebyshev Distance
    """
    return max(abs(position[0] - goal_position[0]), abs(position[1] - goal_position[1]))


def heuristic(position, goal_position):
    return np.linalg.norm(np.array(position) - np.array(goal_position))