Updates and fixes to ROS interface