Added rqt trajectory gui