refactor(goal_planner): refactor goal_searcher and goal_candidates #11407

Job	Run time
cppcheck-differential	59s
	59s

Provide feedback