diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
index e72807da59..dddd38c183 100644
--- a/nav2_costmap_2d/src/costmap_2d.cpp
+++ b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -265,16 +265,19 @@ unsigned char * Costmap2D::getCharMap() const
 
 unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
 {
+  std::unique_lock<mutex_t> lock(*access_);
   return costmap_[getIndex(mx, my)];
 }
 
 unsigned char Costmap2D::getCost(unsigned int undex) const
 {
+  std::unique_lock<mutex_t> lock(*access_);
   return costmap_[undex];
 }
 
 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
 {
+  std::unique_lock<mutex_t> lock(*access_);
   costmap_[getIndex(mx, my)] = cost;
 }