Improved handling of max step height on sloped terrain

[haider8645]

Hello @chhtz,

here the PR for the change to handling of step height check on sloped terrain.

• We would need to add a dependency to cgal in ugv installation instructions for this change

Best,
Haider

[haider8645]

@m0rsch1 after testing, please let us know if this branch solves the issue of the strange obstacles.

[m0rsch1]

@haider8645 This is a BIG improvement on the map quality and needs to be merged asap 🥇
I would suggest, that you/we later perform a complete review of this library to even further improve this.

[m0rsch1]

[haider8645]

@chhtz has some ideas of improving the polygon intersection with the MLS. We could discuss that as the next step as one of the ways to improve the performance.

[haider8645]

I am now writing some tests so that we can improve the performance and also compare the time taken for the trav map expansion scaling with map size and number of collisions

[haider8645]