A step towards mapped based navigation, how?

edit

Hi,

I have implemented a pretty basic system that utilises a GNSS receiver (with RTK) for navigation and 2D LIDAR's (all around the vehicle) for collision avoidance.

I am now trying to move towards something like the navigation stack, something that is able to navigate through obstacles. I am somewhat familiar with TF's and frames however I am not sure which direction to move on.

My current, very basic idea is:

1. Create a map of obstacles in latitude and longitude plane (essentially a costmap) using the LIDAR's (from LEDDARTECH to be exact).
2. Use GNSS coordinates (latitude and longitude) for localisation instead of odometry.
3. Create a path planner to navigate through the map with recovery mechanisms.

From my perspective, I don't see why this wouldn't work if implemented correctly, however I am not sure where to begin.

• Should I bring in all the sensors to the same coordinate space using TF?
• Are there any references as to how costmaps are actually computed?
• What's the logic behind a path planner navigating through a costmap?

I tried looking at the navigation stack's source, as well as AMCL and SLAM algorithms however they are a bit too difficult for me to grasp at the moment.

Could someone please guide me towards the right direction, or, correct me if I am wrong somewhere?