Added publishing of sonar and imu sensor readings.