diff --git a/costmap_2d/src/costmap_2d.cpp b/costmap_2d/src/costmap_2d.cpp
index fb42b83ba7..9f949d3c80 100644
--- a/costmap_2d/src/costmap_2d.cpp
+++ b/costmap_2d/src/costmap_2d.cpp
@@ -414,7 +414,7 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::ve
 
     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;