From e16a44c65ee7064e2271118894b92bb6e24ce28d Mon Sep 17 00:00:00 2001
From: Leif Terry <leif@peanutrobotics.com>
Date: Wed, 11 Oct 2023 16:38:42 -0400
Subject: [PATCH] Use mutex to protect costmap reads. Otherwise costmap can be
 read during a map update and return 0.

---
 nav2_costmap_2d/src/costmap_2d.cpp | 3 +++
 1 file changed, 3 insertions(+)

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;
 }