ROS2: Path Planning Algorithms Python Implementation

Adding A-Star (Grid) to our Project

A bit of Theory

I am going to do it quick and dirty way: the keepout map (say, 640x640 px) is transformed to a grid (say, 64x64) and a-star algorithm takes this grid as a "map".

The keepout map can be created using map_editor utility. First, "mountains" on the map, swimming pool, river and a house are marked as obstacles (128). Then grass is marked as partially drivable (64). Finally, a "gradient" icon is applied to a road graph, so that roads become fully drivable (0) in the middle (20 px across with extra 5px of gradient from 0 to 64 on each side. This way our robot can move on grass, but will prefer roads, if possible.

As you can see, map_editor utility is not a perfect drawing tool and I am not a perfect artist, but this will do:

Note that the color in combo boxes is limited (0-128), but you can type it directly, too (255).

New keepout map

I do not like the current approach to keepout maps, and will probably change it in future. Right now, the map_editor utility uses range of transparencies 0-128. On one side, it is convenient, as semi-transparent keepout map allows us to see the ground. On the other hand, I would like to use a full range, up to 255.

Notice that I have placed couple of objects on top of a background image. The reason is, AMCL localization algorithm uses lidar scan data, so we need to provide something lidar can see.

Of course, these objects are located on top of their icons on the background image:

There are additional changes, compared to previous section. Now (and this is probably the way it should be) the Coordinator class has two planners: grid planner and graph planner. It is not user right now, but will be used soon: the idea is to use grid planner to get to the road, if the initial robot position is off the graph. Obviously, using grid planner is better than going by straight line (most likely, to the nearest ditch).

If grid planner is set to None, the straight line approach will be used.


This is a planner that uses a-star grid algorithm. It is derived from AStar class (see earlier sections). Now, it DEFINITELY has to be also derived from GlobalPlannerStraightLine class, as it uses straight line functions. I will do it later.

Also, as we have two planners now, is renamed to


The path follower file didn't change at all, compared to earlier section.


(C), all rights reserved

Please read the disclaimer