Trajectory marker

This adds the possibility to publish a visualization marker array with the trajectories generated.
They are published if `publish_trajectories` param is set to `True` (available on the dynamic reconfigure `cfg` file). The trajectory markers are separated in different namespaces, depending on the logic that generates them.

This implementation shows only the trajectory endpoint; I think that drawing all the trajectory points is not really useful and shows so many points. The trajectory cost is not drawn because there is no clear upper bound for the conversion from cost to RGB.
I'm open to any advice to improve it.

The PR comes with several commits that removes empty lines, trailing white spaces and unused code or trivial changes.

@RainCT This is inspired in some implementation you did to handle markers and draw the trajectories. I've tried to keep the changes minimal, but maybe I should use your implementation at the end, if you agree.