Start writing goal_publisher.py