Lock costmap in getCostsCallback

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -831,6 +831,7 @@ void Costmap2DROS::getCostsCallback(
   unsigned int mx, my;
 
   Costmap2D * costmap = layered_costmap_->getCostmap();
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap->getMutex()));
   response->success = true;
   for (const auto & pose : request->poses) {
     geometry_msgs::msg::PoseStamped pose_transformed;
@@ -875,6 +876,7 @@ void Costmap2DROS::getCostsCallback(
       response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));
     }
   }
+  lock.unlock();
 }
 
 }  // namespace nav2_costmap_2d