RootRework: General Refactor and Rewrite

[MoffKalast]

After #103 is done and we've got move_smooth to work on, we should do a general rewrite of the node to clean up the code and sort out or at least somewhat address the following issues and add required features:

• reducing fishtailing
• running away from/missing goals
• implementing a 2D PID system

[dorkamotorka]

Why do you want to implement 2D PID system? I assume it's lateral and linear control but that's already done. I forgot but I think we are using Stanley lateral controller and linear is just max acceleration->max speed-> max decceleration.

[MoffKalast]

I'm not sure, I wasn't directly involved with that discussion at the time but that's what I last remember being the plan. We should probably have a meeting to get on the same page about it.