Added graph publisher to visualize it on RViz