Created a publish goal function and added some log messages
Just one vehicle active.
Changed velocity parameter.
Updated change.log
Finish v0.6.0