Fix up bugs, basic ROS compatability working.