Merged changes for stomp_ros_interface from IRL branch