diff --git a/include/costmap_2d/utils.h b/include/costmap_2d/utils.h
index 7e17ea7..a61a703 100644
--- a/include/costmap_2d/utils.h
+++ b/include/costmap_2d/utils.h
@@ -28,6 +28,7 @@ namespace costmap_2d
             geometry_msgs::Point point;
             point.x = p[0].as<double>();
             point.y = p[1].as<double>();
+            point.z = 0.0;
             fp.push_back(point);
         }
         std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;