[Navigation] Collision monitor

[KOveres]

Summary

If our robot is driving we don't want it to bump into people or objects.

DOD:

• Check previous work done on collision monitoring, switch and integrate in current navigation solution
• Make a test plan
• Works in simulation on Lynx and Panther
• Works in real life on Lynx and Panther
• Documentation up-to-date
• Reviewed

Additional Context

Related issues, links, or references.