Fixed publishing plan message
Merge branch 'master' of https://github.com/gtagency/ros-self-driving-core