diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index 8c7041bcb7..245323db51 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -325,6 +325,9 @@ DWBLocalPlanner::computeVelocityCommands(
 
   prepareGlobalPlan(pose, transformed_plan, goal_pose);
 
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
   for (TrajectoryCritic::Ptr critic : critics_) {
     if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) {
       RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare");
@@ -344,6 +347,8 @@ DWBLocalPlanner::computeVelocityCommands(
       critic->debrief(cmd_vel.velocity);
     }
 
+    lock.unlock();
+
     pub_->publishLocalPlan(pose.header, best.traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 
@@ -355,6 +360,9 @@ DWBLocalPlanner::computeVelocityCommands(
     for (TrajectoryCritic::Ptr critic : critics_) {
       critic->debrief(empty_cmd);
     }
+
+    lock.unlock();
+
     pub_->publishLocalPlan(pose.header, empty_traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index d291494589..898c1648d7 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -174,6 +174,8 @@ NavfnPlanner::makePlan(
   // clear the starting cell within the costmap because we know it can't be an obstacle
   clearRobotCell(mx, my);
 
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+
   // make sure to resize the underlying array that Navfn uses
   planner_->setNavArr(
     costmap_->getSizeInCellsX(),
@@ -181,6 +183,8 @@ NavfnPlanner::makePlan(
 
   planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
 
+  lock.unlock();
+
   int map_start[2];
   map_start[0] = mx;
   map_start[1] = my;