Writing a basic real-time joint controller in ROS