more cleanup
adding a state machine
added a simple state machine to manage agents
integrating the state machine, had to move some things around and also fixed/optimized some pieces in the process
Merge branch 'master' of github.com:srl-freiburg/pedsim_ros