Merging set_pose service addition into hydro_devel