diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
index 2074ad5ea60..772ede34c70 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
@@ -71,7 +71,8 @@ class Costmap2DPublisher
     Costmap2D * costmap,
     std::string global_frame,
     std::string topic_name,
-    bool always_send_full_costmap = false);
+    bool always_send_full_costmap = false,
+    float map_vis_z = 0.0);
 
   /**
    * @brief  Destructor
@@ -156,6 +157,7 @@ class Costmap2DPublisher
   double saved_origin_y_;
   bool active_;
   bool always_send_full_costmap_;
+  float map_vis_z_;
 
   // Publisher for translated costmap values as msg::OccupancyGrid used in visualization
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index 95d2ef26732..a5716fb0a25 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -380,6 +380,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   bool rolling_window_{false};     ///< Whether to use a rolling window version of the costmap
   bool track_unknown_space_{false};
   double transform_tolerance_{0};  ///< The timeout before transform errors
+  float map_vis_z_{0};
 
   // Derived parameters
   bool use_radius_{false};
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index 96d98c3d377..8874cbfd922 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -54,12 +54,14 @@ Costmap2DPublisher::Costmap2DPublisher(
   Costmap2D * costmap,
   std::string global_frame,
   std::string topic_name,
-  bool always_send_full_costmap)
+  bool always_send_full_costmap,
+  float map_vis_z)
 : costmap_(costmap),
   global_frame_(global_frame),
   topic_name_(topic_name),
   active_(false),
-  always_send_full_costmap_(always_send_full_costmap)
+  always_send_full_costmap_(always_send_full_costmap),
+  map_vis_z_(map_vis_z)
 {
   auto node = parent.lock();
   clock_ = node->get_clock();
@@ -153,7 +155,7 @@ void Costmap2DPublisher::prepareGrid()
   costmap_->mapToWorld(0, 0, wx, wy);
   grid_->info.origin.position.x = wx - grid_resolution / 2;
   grid_->info.origin.position.y = wy - grid_resolution / 2;
-  grid_->info.origin.position.z = 0.0;
+  grid_->info.origin.position.z = map_vis_z_;
   grid_->info.origin.orientation.w = 1.0;
   saved_origin_x_ = costmap_->getOriginX();
   saved_origin_y_ = costmap_->getOriginY();
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index cf264429c0d..cec8396100f 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -101,7 +101,7 @@ Costmap2DROS::Costmap2DROS(
 {
   declare_parameter(
     "map_topic", rclcpp::ParameterValue(
-      (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map")));
+      (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); 
   init();
 }
 
@@ -112,6 +112,7 @@ void Costmap2DROS::init()
   std::vector<std::string> clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"};
 
   declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
+  declare_parameter("map_vis_z", rclcpp::ParameterValue(0.0));
   declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
   declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]")));
   declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
@@ -221,7 +222,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   costmap_publisher_ = std::make_unique<Costmap2DPublisher>(
     shared_from_this(),
     layered_costmap_->getCostmap(), global_frame_,
-    "costmap", always_send_full_costmap_);
+    "costmap", always_send_full_costmap_, map_vis_z_);
 
   auto layers = layered_costmap_->getPlugins();
 
@@ -232,7 +233,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
         std::make_unique<Costmap2DPublisher>(
           shared_from_this(),
           costmap_layer.get(), global_frame_,
-          layer->getName(), always_send_full_costmap_)
+          layer->getName(), always_send_full_costmap_, map_vis_z_)
       );
     }
   }
@@ -368,6 +369,7 @@ Costmap2DROS::getParameters()
 
   // Get all of the required parameters
   get_parameter("always_send_full_costmap", always_send_full_costmap_);
+  get_parameter("map_vis_z", map_vis_z_);
   get_parameter("footprint", footprint_);
   get_parameter("footprint_padding", footprint_padding_);
   get_parameter("global_frame", global_frame_);