diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
index c2edd13b6e2..cb0cece1937 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
@@ -45,9 +45,9 @@ inline geometry_msgs::msg::Pose getWorldCoords(
 {
   geometry_msgs::msg::Pose msg;
   msg.position.x =
-    static_cast<float>(costmap->getOriginX()) + (mx - 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution();
   msg.position.y =
-    static_cast<float>(costmap->getOriginY()) + (my - 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution();
   return msg;
 }