Vision working: can test for robot presence, assign traces, publish tfs. Camera needs recalibration and calibration routine. Also needs exporting parameter to dynamic reconfigure and parameter server.