ros node mostly filled in now
add stability visualization