Adding a simple goal publisher for rViz