Clean the code

[leomichalski]

The flight control code has some smart tricks but is a complete mess. It needs to be refactored into multiple .cpp and .hpp files. It also needs more methods and, maybe, classes (maybe c-style classes).

The vision code (Python) is almost pseudo-code but is poorly documented beyond that. It is weird to keep the "GazeboVideoSource" class inside the "rosbridge.py" file when there is a file named "videosource.py", refactor that.

I won't even write about the "scripts" wtf folder.

Tasks:

• Refactor the flight control C++ code.
• Stop hardcoding everything (takeoff height, waypoints, etc) in the aruco_hunter package. Use a roslaunch file and calculate the waypoints before taking off.
• Document the vision code properly.
• Remove some code smells from the vision code.
• Clean the scripts folder.