Added mp2 source code

src/mp2/CMakeLists.txt

src/mp2/launch/mp2.launch

src/mp2/package.xml

src/mp2/src/controller.py

src/mp2/src/main.py

src/mp2/src/set_pos.py

src/mp2/src/util.py

src/mp2/src/waypoint_list.py