Add output to screen to launch file and some debugging statements/slight changes in StateMachine.