Update footstep marker

update footstep marker to publish snapped posestamped to plane