Finished pose integrator class, teleop task node controls depth now.