diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
index 5e963c4f977fed0c9f964619be746c7b2c3965b7..764496cc68956b12c0243540f573e3cc0c444050 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -152,6 +152,11 @@ private:
   {
     return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
   }
+
+  double getSizeInMetersZ() const
+  {
+    return (size_z_ - 1 + 0.5) * z_resolution_;
+  }
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 526b6200f4faee854f5d6c9e9a3ac3b81f8854c1..24f2b6faede3007ae116bb9fee88f1944eb8b2fb 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -314,6 +314,7 @@ void VoxelLayer::raytraceFreespace(
   // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
   double map_end_x = origin_x_ + getSizeInMetersX();
   double map_end_y = origin_y_ + getSizeInMetersY();
+  double map_end_z = origin_z_ + getSizeInMetersZ();
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
@@ -337,9 +338,9 @@ void VoxelLayer::raytraceFreespace(
     double t = 1.0;
 
     // we can only raytrace to a maximum z height
-    if (wpz > max_obstacle_height_) {
+    if (wpz > map_end_z) {
       // we know we want the vector's z value to be max_z
-      t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
+      t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
     } else if (wpz < origin_z_) {
       // and we can only raytrace down to the floor
       // we know we want the vector's z value to be 0.0