Merging set_pose service addition into indigo_devel