[hector_exploration_planner] Minor optimizations, cleanup