should be working now just have to test it with the turtlebot