diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index b849e89908..7a5f8d4b7f 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -62,6 +62,9 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer
 
   void setCostmap(nav2_util::Costmap * costmap)
   {
+    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(
+      *(costmap_ros_->getCostmap()->getMutex()));
+
     nav2_msgs::msg::CostmapMetaData prop;
     nav2_msgs::msg::Costmap cm = costmap->get_costmap(prop);
     prop = cm.metadata;