Added sensor simulator code
Redid sensor simulator functions to prepare for integration into ROS.