fix possible use-after-free: unsafe shared_ptr in multithread (#2530)

Co-authored-by: Kai-Tao Xie <kaitaoxie@qq.com>

nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -55,38 +55,41 @@ 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;
     }
   }
 }
 
 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;
   }