PedSim simulator (really ugly ROSification, needs major cleaning)