intention_modelling.cpp now publish correct current trajectory points