Introduced locks for preventing preemption, added output, added pose validity checks