The code for this section is located in multi_bot_nav_03 part of an archive. Note that archive includes some additional folders, like maps and worlds, that are used by all projects and therefore are located outside of them. Also note that we use ROS2 Galactic.
A* algorithm is a faster alternative to Bellman-Ford and is widely used to find a shortest path in a graph. In this section I am going to provide two versions of code for two different scenarios of use.
The objective is to find the optimal path from start point to a destination point taking into consideration distance and road conditions (or "price") associated with a particular path (dirt roads vs highway or toll roads vs free ones).
A* algorithm is one of the most used for navigation, as it is reasonably fast, accurate and customizable. In this section, I am going to implement two versions of A-star algorithm, one for an "uncharted" land, and one for an existing road network.
Also note, that in both cases I am going to use weights and tresholds. Treshold is a level of "road quality": if it is below certain level, we shouldn't drive across this place. To deal with it, we are going to use the keepout map, similar to what was explained in the Nav2 section.
As for weights, they are "soft" version of treshold: the higher the "drivability" of a road, the higher are the chances that the robot will choose it. Think about choosing between the highway and a dirt road: if there is no choice, we plot our rout over dirt road, but generally, we prefer highways.
To deal with path planning, A* algorithm represents the map as a graph. It allows us to use variant of a breath first search, that is very well formalized and fine tuned.
A graph is a set of nodes (locations) and edges that connect them. In our case, it takes different nodes to copnnect A to B vs B to A, in other words, we can have one-way streets or streets that have different quality in opposite directions.
The output is a path, which is a list of nodes we have to visit, one after another, to get from A to B.
Note that the version of an algorithm that I provide here does not work if there are two segments both going from A to B. To distinguish them, we can simply insert an extra node, so we instead of [(A, B), (A, B)], we have [(A, C, B), (A,B)].
Also note, that our graph can have cycles, but can not have nodes with negative weights.
BFS, Dijkstra, A-star
If you ever used LeetCode.com to prepare for a job interview, you
should be familiar with BFS algorithm. The idea, when we apply it to
navigation, is to expand all possible paths from the starting point, in
all directions simultaneously, until we reach the destination.
Dijkstra algorithm improves over the pure BFS: it takes into consideration the "price" of a particular route, prefering "cheaper" alternatives.
Finally, A* algorithm improves over Dijkstra, by using "hints". While Dijkstra algorithm is great for finding paths to ALL destinations, when we are only interested to finding path to one destination only, why not to use the straight line (or Manchester, or whatever) distance to the destination to give preference to points that are closer to it? This way the "tree" we build grows towards destination, and not in all directions at once.
All three mentioned algorithms use the idea of a "frontier" - a ring of nodes separating nodes we already explored from those we haven't visited yet. To use this idea, we add the starting point to "not visited" list, then we move it to "visited" list and add its neighbours (only ones not in "visited" list) to the "not visited" list. Then we do the same for each neighbour - the ring is expanding.
As we expand the frontier, we keep track of the "parent" node of each node, the one we came to this node from. After we find the destination, we can backtrack our steps by simply moving through the list (in our case it is called "come_from") in the opposit direction:
As for Dijkstra, it adds the price of getting from one node to another. From programmer's point of view, it means using some kind of a priority queue instead of a regular queue. We want to prioritize path that cost LESS.
Finally, A* algorithm uses heiristic search so that the frontier expands towards the goal. All it takes is using the distance (like in my code) or a Manhattan distance (as in the following example:)
This was a very short introduction. If you want to get more ideas about directions such code can evolve, take a look at an exellent Map Representations Web page (more in list of links below).