added collision testing for pushed objects via offset of robot
silenced most publishers inside the planning loop for speed