Refactoring, weird rviz issues with the graph