Fix missing cost consideration in first expansion of SmacPlannerHybri…#5670

wang-fire wants to merge 1 commit intoros-navigation:mainros-navigation/navigation2:mainfrom

wang-fire:fix-first-node-expansionwang-fire/navigation2:fix-first-node-expansionCopy head branch name to clipboard