ROS publishing and minor fixes
ROS publishing and minor fixes