how to find a path from source to target with necessary points constraint by A star algorithm

Question

Using A star algorithm， it is easy to calculate the shortest path from the source point to target point. What if we add the constraint of necessary points？ How to solve it ?

necessary points is a set of points on the path from the source point to target point that must be passed in order.

For example, given source point A ,target point B and necessary points C,D,E the possible result path is A->C->...->D->..._>E->...->B and A->G->...->H->B is not expect result ,because this path does not include necessary points

By the way the path is simple path, which means there are no loops in the path

path showing in picture

Answer 1

Conflict-Based Search (CBS) can be used to find a path from source to target with necessary points.

The main structure of CBS is Constraint Tree (CT). Each node of the tree has the following attributes:

1. solution - list of paths. For example, if we have source point A, target point B and necessary points C,D,E, then our solution looks like [path A->..->C, path C->..->D, path D->..->E, path E->..->B].
2. constraints - list of constraints for each part of our solution
3. cost - sum of length of our solution

First, we create a root node without constraints. We can find all parts of the node solution using some kind of pathfinding algorithm, for example A*. At this moment, we don't care about collisions.

Then we performs a best-first search on the CT where nodes are ordered by their costs. When we process a node, we check if there are collisions in the node solution.

If there are no collisions, then we have found a solution, our path is simple and has no loops.

If there are collisions, we select one of the collisions and create two inherited nodes. Each inherited node inherits the constraints of the parent node and adds one new constraint according to the collision. For each inherited node we find a new solution that consistent with the new constraints.

For example if we found a collision (p1, p2, v). Where p1 - is one part of the node solution and p2 - is another part of the solution, and v is a vertex. Then we create left child with a new additional constraint: part p1 cannot use vertex v. And right child with a new additional constraint: part p2 cannot use vertex v.

We performs best-first search on the CT until we find a collision-free node.

Implementation in python (python 3.10, pathfinding==1.0.10):

import time
import heapq
from copy import deepcopy
from pathfinding.core.grid import Grid, DiagonalMovement
from pathfinding.finder.a_star import AStarFinder


class CTNode:
    def __init__(self, solution, constraints):
        self.solution = solution
        self.constraints = constraints

    def cost(self):
        return sum(len(path) for path in self.solution)

    def find_conflict(self):
        for p1, path1 in enumerate(self.solution):
            for p2, path2 in enumerate(self.solution[p1 + 1 :], start=p1 + 1):
                if p2 == p1 + 1:
                    path2 = path2[1:]
                for v in path1:
                    if v in path2:
                        return [p1, p2], v

    def path(self):
        path = []
        for p in self.solution:
            path += p[:-1]
        path.append(self.solution[-1][-1])
        return path

    def __lt__(self, other):
        return self.cost() < other.cost()


def low_level(grid, start, goal, constraints=None):
    grid = deepcopy(grid)
    if constraints:
        for c in constraints:
            grid.node(*c).walkable = False

    finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
    path, _ = finder.find_path(grid.node(*start), grid.node(*goal), grid)
    return [(node.x, node.y) for node in path]


def resolve_conflict(grid, start, goal, node, part_id, conflict_point):
    if conflict_point == start or conflict_point == goal:
        # impossible to resolve this
        return

    constraints = deepcopy(node.constraints)
    constraints[part_id].append(conflict_point)

    new_path = low_level(grid, start, goal, constraints[part_id])
    if not new_path:
        return

    solution = deepcopy(node.solution)
    solution[part_id] = new_path
    return CTNode(solution, constraints)


def create_root_node(grid, points):
    solution, constraints = [], []
    for start, goal in zip(points[:-1], points[1:]):
        path = low_level(grid, start, goal)
        if not path:
            return
        solution.append(path)
        constraints.append([])

    return CTNode(solution, constraints)


def cbs(grid: Grid, necessary_points: list[tuple], max_time=10):
    root = create_root_node(grid, necessary_points)
    if not root:
        return
    queue = [root]
    heapq.heapify(queue)

    start_time = time.time()
    while queue:
        node = heapq.heappop(queue)

        conflict = node.find_conflict()
        if not conflict:
            return node.path()

        conflicting_parts, conflict_point = conflict
        for part_id in conflicting_parts:
            start, goal = necessary_points[part_id : part_id + 2]
            new_node = resolve_conflict(
                grid, start, goal, node, part_id, conflict_point
            )
            if new_node:
                heapq.heappush(queue, new_node)

        if time.time() - start_time > max_time:
            raise TimeoutError()


if __name__ == "__main__":
    grid = Grid(
        matrix=[
            [1, -1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1],
            [-1, 1, -1, -1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1],
        ]
    )
    necessary_points = [(0, 0), (5, 4), (1, 5), (2, 0)]
    path = cbs(grid, necessary_points)
    print(path)

Result