class AStarGridDynamic

Introduction

The A* Grid Path Planner introduced earlier is a great illustration of an A* algorithm, but it is not very practical. Let's say we have a map with side length equal one kilometer. And we want our navigation to be accurate within one meter. That gives us 1000 x 1000 = 1,000,000 cells and the algorithm becomes unpractically slow.

In the same time, if we use larger cells, the objects that are smaller than the cell size might be ignored. For example, the only bridge across the river. And the path planning will fail.

A solution is quite obvious: use larger cells when the area is painted same color, and smaller cells in the areas where colors change. That reduces required number of cells dramatically. This is usually refered to as A* Grid Algorithm with Dynamic Cell Sizes

Of course, same algorithm works in Gazebo:

Code walkthrough

The idea is: if the cell (of a .pgm file that is used as a keepout map, same as in the rest of ROS2) contains different enough colors (for example, there is a wall dividing an otherwice driveable cell) we divide this cell into four cells. Unless we reached the minimum cell size, in which case we assign it the color of a lowect pixel of a cell. It means that if the 1x1 m cell contains white background (driveable) and black circle for a projection of a lamp post (non-driveable), the resulting color is black.

As cell size is always divided by two, original cell size should be power of two (like 256 meters) or there will be artifacts and an algorithm will fail.

As before, the file has its own "main" function, and can be tested separately, without ROS2.

AStarGridDynamic.py

This algorithm can be improved, and I will do some code changes in future. For example, there is no need to mark the areas where robots will never go, and amount of cells can be reduced by changing the way we allocate them.