also store the handle in the goal_map as a std::pair
check for succeeded motion objectives and remove them