diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
index b21c493590f..7255306252a 100644
--- a/nav2_costmap_2d/src/costmap_2d.cpp
+++ b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -467,7 +467,7 @@ void Costmap2D::convexFillCells(
 
     MapLocation pt;
     // loop though cells in the column
-    for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
+    for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) {
       pt.x = x;
       pt.y = y;
       polygon_cells.push_back(pt);