Added success message

Added success message when trajectory is correct