diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp
index 305f4141f3c..406ca377c3f 100644
--- a/nav2_constrained_smoother/src/constrained_smoother.cpp
+++ b/nav2_constrained_smoother/src/constrained_smoother.cpp
@@ -132,6 +132,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat
 
   // Smooth plan
   auto costmap = costmap_sub_->getCostmap();
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
   if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) {
     RCLCPP_WARN(
       logger_,
diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
index 463af33fd08..9a9621f03dc 100644
--- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp
+++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
@@ -82,6 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
   void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
   {
     costmap_msg_ = msg;
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+      msg->metadata.size_x, msg->metadata.size_y,
+      msg->metadata.resolution, msg->metadata.origin.position.x,
+      msg->metadata.origin.position.y);
+
+    processCurrentCostmapMsg();
     costmap_received_ = true;
   }
 };
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..c56dbbd1417 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
@@ -48,6 +48,7 @@
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "map_msgs/msg/occupancy_grid_update.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/msg/costmap_update.hpp"
 #include "nav2_msgs/srv/get_costmap.hpp"
 #include "tf2/transform_datatypes.h"
 #include "nav2_util/lifecycle_node.hpp"
@@ -91,6 +92,7 @@ class Costmap2DPublisher
     costmap_pub_->on_activate();
     costmap_update_pub_->on_activate();
     costmap_raw_pub_->on_activate();
+    costmap_raw_update_pub_->on_activate();
   }
 
   /**
@@ -101,6 +103,7 @@ class Costmap2DPublisher
     costmap_pub_->on_deactivate();
     costmap_update_pub_->on_deactivate();
     costmap_raw_pub_->on_deactivate();
+    costmap_raw_update_pub_->on_deactivate();
   }
 
   /**
@@ -136,6 +139,9 @@ class Costmap2DPublisher
   void prepareGrid();
   void prepareCostmap();
 
+  /** @brief Prepare CostmapUpdate msg for publication. */
+  std::unique_ptr<nav2_msgs::msg::CostmapUpdate> createCostmapUpdateMsg();
+
   /** @brief Publish the latest full costmap to the new subscriber. */
   // void onNewSubscription(const ros::SingleSubscriberPublisher& pub);
 
@@ -164,6 +170,8 @@ class Costmap2DPublisher
 
   // Publisher for raw costmap values as msg::Costmap from layered costmap
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CostmapUpdate>::SharedPtr
+    costmap_raw_update_pub_;
 
   // Service for getting the costmaps
   rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
index e4ea745015d..42e5b9e1210 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/msg/costmap_update.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 
 namespace nav2_costmap_2d
@@ -52,25 +53,36 @@ class CostmapSubscriber
   ~CostmapSubscriber() {}
 
   /**
-   * @brief A Get the costmap from topic
+   * @brief Get current costmap
    */
   std::shared_ptr<Costmap2D> getCostmap();
-
-  /**
-   * @brief Convert an occ grid message into a costmap object
-   */
-  void toCostmap2D();
   /**
    * @brief Callback for the costmap topic
    */
   void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg);
+  /**
+   * @brief Callback for the costmap's update topic
+   */
+  void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg);
 
 protected:
+  void processCurrentCostmapMsg();
+
+  bool haveCostmapParametersChanged();
+  bool hasCostmapSizeChanged();
+  bool hasCostmapResolutionChanged();
+  bool hasCostmapOriginPositionChanged();
+
+  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+  rclcpp::Subscription<nav2_msgs::msg::CostmapUpdate>::SharedPtr costmap_update_sub_;
+
   std::shared_ptr<Costmap2D> costmap_;
   nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
+
   std::string topic_name_;
   bool costmap_received_{false};
-  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+  std::mutex costmap_msg_mutex_;
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")};
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index 87881bc7caa..17d71b84ffa 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -76,6 +76,8 @@ Costmap2DPublisher::Costmap2DPublisher(
     custom_qos);
   costmap_update_pub_ = node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
     topic_name + "_updates", custom_qos);
+  costmap_raw_update_pub_ = node->create_publisher<nav2_msgs::msg::CostmapUpdate>(
+    topic_name + "_raw_updates", custom_qos);
 
   // Create a service that will use the callback function to handle requests.
   costmap_service_ = node->create_service<nav2_msgs::srv::GetCostmap>(
@@ -181,13 +183,30 @@ void Costmap2DPublisher::prepareCostmap()
   }
 }
 
-void Costmap2DPublisher::publishCostmap()
+std::unique_ptr<nav2_msgs::msg::CostmapUpdate> Costmap2DPublisher::createCostmapUpdateMsg()
 {
-  if (costmap_raw_pub_->get_subscription_count() > 0) {
-    prepareCostmap();
-    costmap_raw_pub_->publish(std::move(costmap_raw_));
+  auto msg = std::make_unique<nav2_msgs::msg::CostmapUpdate>();
+
+  msg->header.stamp = clock_->now();
+  msg->header.frame_id = global_frame_;
+  msg->x = x0_;
+  msg->y = y0_;
+  msg->size_x = xn_ - x0_;
+  msg->size_y = yn_ - y0_;
+  msg->data.resize(msg->size_x * msg->size_y);
+
+  std::uint32_t i = 0;
+  for (std::uint32_t y = y0_; y < yn_; y++) {
+    for (std::uint32_t x = x0_; x < xn_; x++) {
+      //todo copy from array instead of calling method
+      msg->data[i++] = costmap_->getCost(x, y);
+    }
   }
+  return msg;
+}
 
+void Costmap2DPublisher::publishCostmap()
+{
   float resolution = costmap_->getResolution();
   if (always_send_full_costmap_ || grid_resolution != resolution ||
     grid_width != costmap_->getSizeInCellsX() ||
@@ -199,10 +218,14 @@ void Costmap2DPublisher::publishCostmap()
       prepareGrid();
       costmap_pub_->publish(std::move(grid_));
     }
+    if (costmap_raw_pub_->get_subscription_count() > 0) {
+      prepareCostmap();
+      costmap_raw_pub_->publish(std::move(costmap_raw_));
+    }
   } else if (x0_ < xn_) {
+    // Publish Just Updates
+    std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
     if (costmap_update_pub_->get_subscription_count() > 0) {
-      std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
-      // Publish Just an Update
       auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>();
       update->header.stamp = rclcpp::Time();
       update->header.frame_id = global_frame_;
@@ -220,6 +243,9 @@ void Costmap2DPublisher::publishCostmap()
       }
       costmap_update_pub_->publish(std::move(update));
     }
+    if (costmap_raw_update_pub_->get_subscription_count() > 0) {
+      costmap_raw_update_pub_->publish(createCostmapUpdateMsg());
+    }
   }
 
   xn_ = yn_ = 0;
diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
index d56ac942e98..f415530af3f 100644
--- a/nav2_costmap_2d/src/costmap_subscriber.cpp
+++ b/nav2_costmap_2d/src/costmap_subscriber.cpp
@@ -14,84 +14,155 @@
 
 #include <string>
 #include <memory>
+#include <mutex>
 
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 
 namespace nav2_costmap_2d
 {
 
+constexpr int costmapUpdateQueueDepth = 10;
+
 CostmapSubscriber::CostmapSubscriber(
   const nav2_util::LifecycleNode::WeakPtr & parent,
   const std::string & topic_name)
 : topic_name_(topic_name)
 {
   auto node = parent.lock();
+  logger_ = node->get_logger();
   costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
     topic_name_,
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
+  costmap_update_sub_ = node->create_subscription<nav2_msgs::msg::CostmapUpdate>(
+    topic_name_ + "_updates",
+    rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(),
+    std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1));
 }
 
+// todo: is it still necessary
 CostmapSubscriber::CostmapSubscriber(
   const rclcpp::Node::WeakPtr & parent,
   const std::string & topic_name)
 : topic_name_(topic_name)
 {
   auto node = parent.lock();
+  logger_ = node->get_logger();
   costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
     topic_name_,
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
+  costmap_update_sub_ = node->create_subscription<nav2_msgs::msg::CostmapUpdate>(
+    topic_name_ + "_updates",
+    rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(),
+    std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1));
 }
 
+// todo: change to const?
 std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
 {
   if (!costmap_received_) {
     throw std::runtime_error("Costmap is not available");
   }
-  toCostmap2D();
+  if (costmap_msg_ != nullptr) {
+    processCurrentCostmapMsg();
+  }
   return costmap_;
 }
 
-void CostmapSubscriber::toCostmap2D()
+void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
 {
-  auto current_costmap_msg = std::atomic_load(&costmap_msg_);
-
-  if (costmap_ == nullptr) {
-    costmap_ = std::make_shared<Costmap2D>(
-      current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
-      current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x,
-      current_costmap_msg->metadata.origin.position.y);
-  } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x ||  // NOLINT
-    costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y ||
-    costmap_->getResolution() != current_costmap_msg->metadata.resolution ||
-    costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x ||
-    costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y)
   {
-    // Update the size of the costmap
-    costmap_->resizeMap(
-      current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
-      current_costmap_msg->metadata.resolution,
-      current_costmap_msg->metadata.origin.position.x,
-      current_costmap_msg->metadata.origin.position.y);
+    std::lock_guard<std::mutex> lock(costmap_msg_mutex_);
+    costmap_msg_ = msg;
   }
+  if (!costmap_received_) {
+    costmap_ = std::make_shared<Costmap2D>(
+      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 < current_costmap_msg->metadata.size_x; ++i) {
-    for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) {
-      master_array[index] = current_costmap_msg->data[index];
-      ++index;
+    processCurrentCostmapMsg();
+
+    costmap_received_ = true;
+  }
+}
+
+void CostmapSubscriber::costmapUpdateCallback(
+  const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg)
+{
+  if (costmap_received_) {
+    if (costmap_msg_) {
+      processCurrentCostmapMsg();
     }
+
+    std::lock_guard<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+
+    auto map_cell_size_x = costmap_->getSizeInCellsX();
+    auto map_call_size_y = costmap_->getSizeInCellsY();
+
+    if (map_cell_size_x < update_msg->x + update_msg->size_x ||
+      map_call_size_y < update_msg->y + update_msg->size_y)
+    {
+      RCLCPP_WARN(
+        logger_, "Update area outside of original map area. Costmap bounds: %d X %d, "
+        "Update origin: %d, %d  bounds: %d X %d", map_cell_size_x, map_call_size_y,
+        update_msg->x, update_msg->y, update_msg->size_x, update_msg->size_y);
+      return;
+    }
+    unsigned char * master_array = costmap_->getCharMap();
+    // copy update msg row-wise
+    for (size_t y = 0; y < update_msg->size_y; ++y) {
+      auto starting_index_of_row_update_in_costmap = (y + update_msg->y) * map_cell_size_x +
+        update_msg->x;
+
+      std::copy_n(
+        update_msg->data.begin() + (y * update_msg->size_x),
+        update_msg->size_x, &master_array[starting_index_of_row_update_in_costmap]);
+    }
+  } else {
+    RCLCPP_WARN(logger_, "No costmap received.");
   }
 }
 
-void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
+void CostmapSubscriber::processCurrentCostmapMsg()
 {
-  std::atomic_store(&costmap_msg_, msg);
-  if (!costmap_received_) {
-    costmap_received_ = true;
+  std::scoped_lock lock(*(costmap_->getMutex()), costmap_msg_mutex_);
+  if (haveCostmapParametersChanged()) {
+    costmap_->resizeMap(
+      costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y,
+      costmap_msg_->metadata.resolution,
+      costmap_msg_->metadata.origin.position.x,
+      costmap_msg_->metadata.origin.position.y);
   }
+
+  unsigned char * master_array = costmap_->getCharMap();
+  std::copy(costmap_msg_->data.begin(), costmap_msg_->data.end(), master_array);
+  costmap_msg_ = nullptr;
+}
+
+bool CostmapSubscriber::haveCostmapParametersChanged()
+{
+  return hasCostmapSizeChanged() ||
+         hasCostmapResolutionChanged() ||
+         hasCostmapOriginPositionChanged();
+}
+
+bool CostmapSubscriber::hasCostmapSizeChanged()
+{
+  return costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x ||
+         costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y;
+}
+
+bool CostmapSubscriber::hasCostmapResolutionChanged()
+{
+  return costmap_->getResolution() != costmap_msg_->metadata.resolution;
+}
+
+bool CostmapSubscriber::hasCostmapOriginPositionChanged()
+{
+  return costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x ||
+         costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y;
 }
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt
index 0768972a0bb..868b5081a1b 100644
--- a/nav2_costmap_2d/test/integration/CMakeLists.txt
+++ b/nav2_costmap_2d/test/integration/CMakeLists.txt
@@ -76,6 +76,17 @@ target_link_libraries(test_costmap_publisher_exec
     layers
 )
 
+ament_add_gtest_executable(test_costmap_subscriber_exec
+test_costmap_subscriber.cpp
+)
+ament_target_dependencies(test_costmap_subscriber_exec
+    ${dependencies}
+)
+target_link_libraries(test_costmap_subscriber_exec
+    nav2_costmap_2d_core
+    nav2_costmap_2d_client
+)
+
 ament_add_test(test_collision_checker
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
@@ -136,6 +147,16 @@ ament_add_test(test_costmap_publisher_exec
     TEST_EXECUTABLE=$<TARGET_FILE:test_costmap_publisher_exec>
 )
 
+ament_add_test(test_costmap_subscriber_exec
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+    ENV
+    TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_costmap_subscriber_exec>
+)
+
 ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml,
 ## which has a dependency on rosbag playback
 # ament_add_gtest_executable(costmap_tester
diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp
new file mode 100644
index 00000000000..7389bfb4826
--- /dev/null
+++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp
@@ -0,0 +1,150 @@
+#include <gtest/gtest.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestCostmapSubscriberShould : public ::testing::Test
+{
+public:
+  TestCostmapSubscriberShould()
+  : subscriberNode(nav2_util::LifecycleNode::make_shared("test_subscriber"))
+  {
+
+  }
+
+  void SetUp() override
+  {
+    fullCostmapMsgCount = 0;
+    updateCostmapMsgCount = 0;
+
+    topicName = "/costmap";
+
+    subscriberThread = std::thread(
+      [this]()
+      {
+        auto localExecutor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+        executor = localExecutor;
+
+
+        localExecutor->add_node(subscriberNode->get_node_base_interface());
+
+        auto costmap_sub = subscriberNode->create_subscription<nav2_msgs::msg::Costmap>(
+          topicName + "_raw", 10,
+          std::bind(&TestCostmapSubscriberShould::costmapCallback, this, std::placeholders::_1));
+
+        auto costmap_update_sub = subscriberNode->create_subscription<nav2_msgs::msg::CostmapUpdate>(
+          topicName + "_raw_updates", 10, std::bind(
+            &TestCostmapSubscriberShould::costmapUpdateCallback, this,
+            std::placeholders::_1));
+
+        // costmapSubscriber =
+        // std::make_shared<nav2_costmap_2d::CostmapSubscriber>(subscriberNode, topicName + "_raw");
+
+        localExecutor->spin();
+      }
+    );
+  }
+
+  void TearDown() override
+  {
+    if (std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> exec = executor.lock()) {
+      exec->cancel();
+    }
+    subscriberThread.join();
+  }
+
+  void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr)
+  {
+    this->fullCostmapMsgCount++;
+  }
+  void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr)
+  {
+    this->updateCostmapMsgCount++;
+  }
+
+protected:
+  int fullCostmapMsgCount;
+  int updateCostmapMsgCount;
+  std::string topicName;
+
+  std::weak_ptr<nav2_costmap_2d::CostmapSubscriber> costmapSubscriber;
+
+  std::thread subscriberThread;
+  std::weak_ptr<rclcpp::executors::SingleThreadedExecutor> executor;
+  nav2_util::LifecycleNode::SharedPtr subscriberNode;
+
+  rclcpp::Logger logger {rclcpp::get_logger("test_costmap_subscriber_should")};
+};
+
+TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs)
+{
+  std::vector<std::vector<std::uint8_t>> expectedCostmaps, recievedCostmaps;
+
+  bool always_send_full_costmap = true;
+
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2D>(10, 10, 1.0, 0.0, 0.0);
+
+  nav2_util::LifecycleNode::SharedPtr publisherNode = nav2_util::LifecycleNode::make_shared(
+    "test_publisher");
+
+  auto costmapPublisher = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
+    publisherNode, costmap.get(), "", topicName, always_send_full_costmap);
+
+  costmapPublisher->on_activate();
+
+  costmapPublisher->publishCostmap();
+  rclcpp::Rate(0.1).sleep();
+
+  costmapPublisher->publishCostmap();
+  rclcpp::Rate(0.1).sleep();
+
+  costmapPublisher->publishCostmap();
+  rclcpp::Rate(0.1).sleep();
+
+  ASSERT_EQ(fullCostmapMsgCount, 3);
+  ASSERT_EQ(updateCostmapMsgCount, 0);
+
+  costmapPublisher->on_deactivate();
+}
+
+// TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs)
+// {
+//   // std::vector<std::vector<std::uint8_t>> expectedCostmaps,recievedCostmaps;
+
+//   // bool always_send_full_costmap = true;
+
+//   // auto costmap = std::make_shared<nav2_costmap_2d::Costmap2D>(10, 10, 1.0, 0.0, 0.0);
+
+//   // nav2_util::LifecycleNode::SharedPtr publisherNode = nav2_util::LifecycleNode::make_shared(
+//   //   "test_publisher_2");
+
+//   // auto costmapPublisher = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
+//   //   publisherNode, costmap.get(), "", topicName, always_send_full_costmap);
+
+//   // costmapPublisher->on_activate();
+
+//   // costmapPublisher->publishCostmap();
+//   // rclcpp::Rate(0.1).sleep();
+
+//   // costmapPublisher->publishCostmap();
+//   // rclcpp::Rate(0.1).sleep();
+
+//   // costmapPublisher->publishCostmap();
+//   // rclcpp::Rate(0.1).sleep();
+
+//   // ASSERT_EQ(fullCostmapMsgCount, 3);
+//   // ASSERT_EQ(updateCostmapMsgCount, 0);
+
+//   // costmapPublisher->on_deactivate();
+
+//   ASSERT_TRUE(false);
+// }
diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
index 466fd015c87..34a319c6c18 100644
--- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
@@ -63,6 +63,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
   void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
   {
     costmap_msg_ = msg;
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+      msg->metadata.size_x, msg->metadata.size_y,
+      msg->metadata.resolution, msg->metadata.origin.position.x,
+      msg->metadata.origin.position.y);
+
+    processCurrentCostmapMsg();
     costmap_received_ = true;
   }
 };
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 836f7219df5..e9750b87119 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CollisionDetectorState.msg"
   "msg/Costmap.msg"
   "msg/CostmapMetaData.msg"
+  "msg/CostmapUpdate.msg"
   "msg/CostmapFilterInfo.msg"
   "msg/SpeedLimit.msg"
   "msg/VoxelGrid.msg"
diff --git a/nav2_msgs/msg/CostmapUpdate.msg b/nav2_msgs/msg/CostmapUpdate.msg
new file mode 100644
index 00000000000..3c865d326ba
--- /dev/null
+++ b/nav2_msgs/msg/CostmapUpdate.msg
@@ -0,0 +1,11 @@
+# Update msg for Costmap containing the modified part of Costmap
+std_msgs/Header header
+
+uint32 x
+uint32 y
+
+uint32 size_x
+uint32 size_y
+
+# The cost data, in row-major order, starting with (x,y) from 0-255 in Costmap format rather than OccupancyGrid 0-100.
+uint8[] data
diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp
index 29f3b89a828..0b494d96c6e 100644
--- a/nav2_smoother/src/simple_smoother.cpp
+++ b/nav2_smoother/src/simple_smoother.cpp
@@ -72,6 +72,8 @@ bool SimpleSmoother::smooth(
 
   std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
 
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
   for (unsigned int i = 0; i != path_segments.size(); i++) {
     if (path_segments[i].end - path_segments[i].start > 9) {
       // Populate path segment
diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp
index 08e475de8e1..9138db7a6ab 100644
--- a/nav2_smoother/test/test_smoother_server.cpp
+++ b/nav2_smoother/test/test_smoother_server.cpp
@@ -148,6 +148,13 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
   void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
   {
     costmap_msg_ = msg;
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+      msg->metadata.size_x, msg->metadata.size_y,
+      msg->metadata.resolution, msg->metadata.origin.position.x,
+      msg->metadata.origin.position.y);
+
+    processCurrentCostmapMsg();
+
     costmap_received_ = true;
   }
 };