Refactored, created robot_viz_tools.h, got MoveIt pick mostly working