Adds wait_for_server and wait_for_server_after_restart

These wait for the ActionServer to be ready and call the specified callback
when it is up. The _after_restart version resets the internal state of the
ActionClient and was intended to be caalled after the node providing the
ActionServer has been restarted, to check when the node has come up fully
and is ready to receive goals.