create simulation rviz and cleaned up code