Add RViz visualization for the whole mobile manipulation