This code is developed for the legendary Robot Igal https://youtu.be/iGOmnC24BvE

Todo List:

1. LocalizationManager is really slow because of the insert of descendantParticles
2. Sometimes the neededYawDelta gets negative and the robot gets in between the tables (one step before the other tables)
3. Check what is the issue with the obstacle avoidance - Maybe we'll need to add AvoidObstacle behavior
4. Fix the issue with the last point - somehow robot Igal get stuck right before it
5. Add processing after the PathPlanner, to fix the waypoints issue
6. Add methods to Manager (methods like init map)
7. Save the planet from the Gay Iranian community missle.