From db803dd88b248cc4ca3dd1cd65784c55e6a1b03d Mon Sep 17 00:00:00 2001
From: Kai-Tao Xie <kaitaoxie@qq.com>
Date: Tue, 24 Aug 2021 20:05:40 +0800
Subject: [PATCH] fix possible use-after-free: unsafe shared_ptr in multithread

---
 nav2_costmap_2d/src/costmap_subscriber.cpp | 35 ++++++++++++----------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
index 37b99bb598..2c12cd104b 100644
--- a/nav2_costmap_2d/src/costmap_subscriber.cpp
+++ b/nav2_costmap_2d/src/costmap_subscriber.cpp
@@ -55,30 +55,33 @@ std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
 
 void CostmapSubscriber::toCostmap2D()
 {
+
+  auto current_costmap_msg = std::atomic_load(&costmap_msg_);
+
   if (costmap_ == nullptr) {
     costmap_ = std::make_shared<Costmap2D>(
-      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);
-  } else if (costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x ||  // NOLINT
-    costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y ||
-    costmap_->getResolution() != costmap_msg_->metadata.resolution ||
-    costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x ||
-    costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y)
+      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(
-      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);
+      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);
   }
 
   unsigned char * master_array = costmap_->getCharMap();
   unsigned int index = 0;
-  for (unsigned int i = 0; i < costmap_msg_->metadata.size_x; ++i) {
-    for (unsigned int j = 0; j < costmap_msg_->metadata.size_y; ++j) {
-      master_array[index] = costmap_msg_->data[index];
+  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;
     }
   }
@@ -86,7 +89,7 @@ void CostmapSubscriber::toCostmap2D()
 
 void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
 {
-  costmap_msg_ = msg;
+  std::atomic_store(&costmap_msg_, msg);
   if (!costmap_received_) {
     costmap_received_ = true;
   }