Fix concurrent access to traversability map by adding mutex

[marco-tranzatto]

This PR addresses the concurrent access of TraversabilityMap within the class TraversabilityEstimation .

Not locking the map was causing exceptions from Grid Map when requesting layers that were temporarily not present (for example "traversability_step", as seen from @mbjelonic ). This happened because the map was updated while a path was checked for traversability, and therefore some layers were updated/substituted while being used from another part of the code.

PR adds the usage of mutex and locks TraversabilityMap within the class TraversabilityEstimation every time it is used. Problem completely disappears, at least in simulation.

[marco-tranzatto]

merging this today if no objections

[martiwer]

Thanks for spotting the mutex error. There is already a mutex for the traversability map (and elevation map) within the TraversabilityMap Class. Would it make sense to use this mutex properly instead of adding a new mutex on a higher level?

[marco-tranzatto]

@martiwer ah, I did not see there was already one. Then yes, I'll use that one

[marco-tranzatto]

@martiwer the mutex is within TraversabililityMap, not TraversabilityEstimation, so I guess you want traversability map to be thread safe instead of the estimation class.

[martiwer]

@marco-tranzatto yes, the idea was to have the TraversabilityMap class thread safe directly if possible. You could also remove the mutex there and add it to the TraversabilityEstimation if this works better.

[marco-tranzatto]

Addressing issue in a different PR