Fix concurrent and consistency issues when adding, removing and moving goals on…

Fix concurrent and consistency issues when adding, removing and moving goals on Entities. Removing local Map and utilize goalSelector directly as ground truth.