Implement parameter setting functions and use velocity mode calculations for /cmd_vel

made initialization more robust... still need to properly handle disconnections while running callback loop.