Cleaned up a bit of code and added a topic subscriber (subscribes to "~stop") which will stop the nao safely if required. Future use of this could include stopping the nao when it's batteries or joints are too hot.