organized namespaces, add rviz node for visualization, improvements