diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index b571f67b60890055508d45053f14a872a8081ba0..7360556acf690f2f7f93a045636038428598248b 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -50,6 +50,7 @@ add_library(nav2_costmap_2d_core SHARED
   src/observation_buffer.cpp
   src/clear_costmap_service.cpp
   src/footprint_subscriber.cpp
+  src/costmap_subscriber.cpp
 )
 
 set(dependencies
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 bf1f61ddaab4baaf2c027633c4871a98fb18072c..cd14eb099433f5bbbf4ac4113da8d6794ed588d9 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
@@ -45,6 +45,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "map_msgs/msg/occupancy_grid_update.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
 #include "tf2/transform_datatypes.h"
 #include "nav2_util/lifecycle_node.hpp"
 
@@ -111,6 +112,7 @@ public:
 private:
   /** @brief Prepare grid_ message for publication. */
   void prepareGrid();
+  void prepareCostmap();
 
   /** @brief Publish the latest full costmap to the new subscriber. */
   // void onNewSubscription(const ros::SingleSubscriberPublisher& pub);
@@ -128,10 +130,14 @@ private:
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
   rclcpp_lifecycle::LifecyclePublisher<map_msgs::msg::OccupancyGridUpdate>::SharedPtr
     costmap_update_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
 
   nav_msgs::msg::OccupancyGrid grid_;
+  nav2_msgs::msg::Costmap costmap_raw_;  
   // Translate from 0-255 values in costmap to -1 to 100 values in message.
   static char * cost_translation_table_;
 };
+
 }  // namespace nav2_costmap_2d
+
 #endif  // NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9686fb4f27b1007f8288dd1e64adbee60716f68
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
@@ -0,0 +1,51 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COSTMAP_2D__COSTMAP_2D_SUBSCRIBER_HPP_
+#define NAV2_COSTMAP_2D__COSTMAP_2D_SUBSCRIBER_HPP_
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class CostmapSubscriber
+{
+public:
+  CostmapSubscriber(
+    rclcpp::Node::SharedPtr ros_node,
+    std::string & topic_name);
+  ~CostmapSubscriber() {}
+
+Costmap2D * getCostmap();
+
+protected:
+  void toCostmap2D();
+  void costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg);
+
+  rclcpp::Node::SharedPtr node_;
+  Costmap2D * costmap_;
+  nav2_msgs::msg::Costmap::SharedPtr msg_;
+  std::string topic_name_;
+  bool costmap_received_;
+  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__COSTMAP_2D_SUBSCRIBER_HPP_
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index cda55300465d7bbd67a9bccfa926c5dd679de7ea..475b9043efc3b7e6a7d18535649ba87a42a55c92 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -59,6 +59,8 @@ Costmap2DPublisher::Costmap2DPublisher(
   // TODO(bpwilcox): port onNewSubscription functionality for publisher
   costmap_pub_ = ros_node->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name,
       custom_qos);
+  costmap_raw_pub_ = ros_node->create_publisher<nav2_msgs::msg::Costmap>(topic_name + "_raw",
+    custom_qos);      
   costmap_update_pub_ = ros_node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
     topic_name + "_updates", custom_qos);
 
@@ -124,8 +126,39 @@ void Costmap2DPublisher::prepareGrid()
   }
 }
 
+void Costmap2DPublisher::prepareCostmap()
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  double resolution = costmap_->getResolution();
+
+  costmap_raw_.header.frame_id = global_frame_;
+  costmap_raw_.header.stamp = node_->now();
+
+  costmap_raw_.metadata.layer = "master";
+  costmap_raw_.metadata.resolution = resolution;
+
+  costmap_raw_.metadata.size_x = costmap_->getSizeInCellsX();
+  costmap_raw_.metadata.size_y = costmap_->getSizeInCellsY();
+
+  double wx, wy;
+  costmap_->mapToWorld(0, 0, wx, wy);
+  costmap_raw_.metadata.origin.position.x = wx - resolution / 2;
+  costmap_raw_.metadata.origin.position.y = wy - resolution / 2;
+  costmap_raw_.metadata.origin.position.z = 0.0;
+  costmap_raw_.metadata.origin.orientation.w = 1.0;
+
+  costmap_raw_.data.resize(costmap_raw_.metadata.size_x * costmap_raw_.metadata.size_y);
+
+  unsigned char * data = costmap_->getCharMap();
+  for (unsigned int i = 0; i < costmap_raw_.data.size(); i++) {
+    costmap_raw_.data[i] = data[i];
+  }
+}
+
 void Costmap2DPublisher::publishCostmap()
 {
+  prepareCostmap();
+  costmap_raw_pub_->publish(costmap_raw_);
   float resolution = costmap_->getResolution();
 
   if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4bed17c2ff59120cc09ea2c3f4f2aee0e1f6620
--- /dev/null
+++ b/nav2_costmap_2d/src/costmap_subscriber.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+
+namespace nav2_costmap_2d
+{
+
+CostmapSubscriber::CostmapSubscriber(
+  rclcpp::Node::SharedPtr ros_node,
+  std::string & topic_name)
+: node_(ros_node),
+  topic_name_(topic_name),
+  costmap_received_(false),
+  costmap_(nullptr)
+{
+  costmap_sub_ = node_->create_subscription<nav2_msgs::msg::Costmap>(topic_name,
+      std::bind(&CostmapSubscriber::costmap_callback, this, std::placeholders::_1));
+}
+
+Costmap2D * CostmapSubscriber::getCostmap()
+{
+  if (costmap_ == nullptr) {
+    throw std::runtime_error("Costmap is not available");
+  }
+  return costmap_;
+}
+
+void CostmapSubscriber::toCostmap2D()
+{
+  if (!costmap_received_) {
+    costmap_ = new Costmap2D(
+    msg_->metadata.size_x, msg_->metadata.size_y,
+    msg_->metadata.resolution, msg_->metadata.origin.position.x,
+    msg_->metadata.origin.position.y);
+  } else if (costmap_->getSizeInCellsX() != msg_->metadata.size_x ||
+    costmap_->getSizeInCellsY() != msg_->metadata.size_y ||
+    costmap_->getResolution() != msg_->metadata.resolution ||
+    costmap_->getOriginX() != msg_->metadata.origin.position.x ||
+    costmap_->getOriginY() != msg_->metadata.origin.position.y)
+  {
+    // Update the size of the costmap 
+    costmap_->resizeMap(msg_->metadata.size_x, msg_->metadata.size_y,
+      msg_->metadata.resolution,
+      msg_->metadata.origin.position.x,
+      msg_->metadata.origin.position.y);
+  }
+
+  unsigned char * master_array = costmap_->getCharMap();
+  unsigned int index = 0;
+  for (unsigned int i = 0; i <  msg_->metadata.size_x; ++i) {
+    for (unsigned int j = 0; j < msg_->metadata.size_y; ++j) {
+      master_array[index] = msg_->data[index];
+      ++index;
+    }
+  }
+}
+
+void CostmapSubscriber::costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg)
+{
+  msg_ = msg;
+  toCostmap2D();
+  if (!costmap_received_) {
+    costmap_received_ = true;
+  }
+}
+
+}  // namespace nav2_costmap_2d
+