Added error handling to goal publishing code