Improve rviz loading time
Merge branch '_rviz_speedup_loading2' into all_dev_combined_baxter