major cleanup and some fixes
throttles error (debugging) msgs
uses gazebo sim time (instead of ros time)