Merged Fix navfn_planner from humble PR #5087 (#5092)

* merged changes from humble for goal.header fix

* reverted back, error in merge

* ported goal.header fix in navfn_planner.cpp from humble

* reverted to navfn_planner.cpp to origin/main

* merged navfn_planner.cpp from humble

* fixed the merge

Author: sandeepdutta

nav2_navfn_planner/src/navfn_planner.cpp

@@ -368,6 +368,7 @@ NavfnPlanner::smoothApproachToGoal(
   }
   geometry_msgs::msg::PoseStamped goal_copy;
   goal_copy.pose = goal;
+  goal_copy.header = plan.header;
   plan.poses.push_back(goal_copy);
 }
 
@@ -417,6 +418,7 @@ NavfnPlanner::getPlanFromPotential(
     mapToWorld(x[i], y[i], world_x, world_y);
 
     geometry_msgs::msg::PoseStamped pose;
+    pose.header = plan.header;
     pose.pose.position.x = world_x;
     pose.pose.position.y = world_y;
     pose.pose.position.z = 0.0;