First working version. Should stop motors on button press. Still not clean. Segfaults when closed because ros::spin is not properly terminated yet.