diff --git a/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp b/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp
index 79504d6..8e7afc8 100644
--- a/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp
+++ b/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp
@@ -832,6 +832,9 @@ mpp::refine_trajectory(plannedPath, pathEdges, planner_input.ptgs);
 		wp_msg.allow_skip = false;
 
 		res.wps.waypoints.push_back(wp_msg);
+
+		res.wps.header.frame_id = frame_id_map_;
+		res.wps.header.stamp = this->now();
 	}
 
 	return res;