safety_controller that stops the robot by publishing empty twists at 500 hz